NBSIR  75-973 

Guide  to  Improving  the 
Performance  of  a  Manipulator 
System  for  Nuclear  Fuel 
Handling  Through  Computer 
Controls 


John  M.  Evans,  Jr.,  Ph.D. 
James  S.  Albus,  Ph.D. 
Anthony  J.  Barbera,  Ph.D. 
Robert  Rosenthal 
William  B.  Truitt 


Office  of  Developmental  Automation  and  Control  Technology 
Institute  for  Computer  Sciences  and  Technology 
National  Bureau  of  Standards 
Washington,  D.  C.  20234 


November,  1 975 
Final  Project  Report 


Prepared  for 


Oak  Ridge  National  Laboratory 
Oak  Ridge,  Tennessee  37830 


NBSIR  75-973 


GUIDE  TO  IMPROVING  THE 
PERFORMANCE  OF  A  MANIPULATOR 
SYSTEM  FOR  NUCLEAR  FUEL 
HANDLING  THROUGH  COMPUTER 
CONTROLS 


John  M.  Evans,  Jr.,  Ph.D. 
James  S.  Albus,  Ph.D. 
Anthony  J.  Barbera,  Ph.D. 
Robert  Rosenthal 
William  B.  Truitt 


Office  of  Developmental  Automation  and  Control  Technology 
Institute  for  Computer  Sciences  and  Technology 
National  Bureau  of  Standards 
Washington,  D.  C.  20234 


November,  1  975 
Final  Project  Report 


Prepared  for 

Oak  Ridge  National  Laboratory 
Oak  Ridge,  Tennessee  37830 


U.S.  DEPARTMENT  OF  COMMERCE,  Rogers  C.B.  Morton,  Secretary 
James  A.  Baker,  III,  Under  Secretary 

Dr.  Betsy  Ancker-Johnson,  Assistarit  Secretary  for  Science  arid  Technology 
NATIONAL  BUREAU  OF  STANDARDS,  Ernest  Ambler.  Acting  Director 


"1,.'  ,  ■ 


TABLE  OF  CONTENTS 


I.  INTRODUCTION 

II.  OPERATIONAL  REQUIREMENTS 

III.  CONTROL  REQUIREMENTS 

IV.  SUMMARY  AND  CONCLUSIONS 

Appendix  A:    LETTER  FROM  AEC  TO  NBS:  NOV.  13,  1974 
Appendix  B:    DESCRIPTION  OF  NBS  COMPUTER  CONTROL  SOFTWARE- 
Appendix  C:    MANIPULATOR/COMPUTER  INTERFACE  DESIGN 
Appendix  D:    INTERFACE  DRIVERS  AND  TEST  SOFTWARE 
Appendix  E:    ROBOT  CONTROL  PROGRAMS 


I.  INTRODUCTION 


The  Office  of  Developmental  Automation  and  Control  Technology  of  the  Institute  for 
Computer  Sciences  and  Technology  of  the  National  Bureau  of  Standards  works  to  assist  other 
Government  agencies  in  the  efficient  application  of  computer  controlled  automation  systems. 
The  products  of  the  program  are  consulting  and  advising  services,  standards  and  guidelines 
on  interface  and  computer  control  systems,  and  performance  specifications  for  the  procure- 
ment and  use  of  computer  controlled  manipulators  and  other  computer  based  automation 
systems.    These  outputs  will  help  other  agencies  and  industry  apply  this  technology  to 
increase  productivity  and  improve  work  quality  by  removing  men  from  hazardous  environments. 

In  FY  1974  personnel  from  the  Oak  Ridge  National  Laboratory  (ORNL)  visited  NBS  to 
discuss  the  feasibility  of  using  computer  control  techniques  to  improve  the  operation  of 
remote  control  manipulators  which  support  their  HTGR  fuel  refabrication  program.  Subsequent 
discussions  led  to  an  agreement  for  NBS  to  develop  a  conceptual  design  for  a  computer 
control  system  for  the  remote  control  electromechanical  manipulators  in  the  Thorium 
Uranium  Re-cycle  Facility  (TURF)  at  ORNL  (See  Appendix  A).    Following  the  above  agreement, 
an  ORNL  decision  was  made  to  assign  the  task  of  developing  the  conceptual  design  to 
ORNL  personnel . 

The  role  of  NBS,  in  support  of  the  ORNL  conceptual  design,  was  shifted  to  responsibility 
for  development  of  complete  computer  programs  for  testing  the  ORNL  computer  interfaces 
and  for  controlling  the  ORNL  manipulator  as  a  robot  in  both  point-to-point  and  continuous 
path  modes.    In  addition,  NBS  agreed  to  serve  as  a  consultant  regarding  the  ORNL  conceptual 
design. 

ORNL  has  provided  the  required  conceptual  design,  and  this  conceptual  design  was 
reviewed  and  endorsed  by  NBS.    Following  agreement  on  the  conceptual  design,  the  required 
computer  programs  to  implement  it  were  developed  at  NBS.    These  programs  are  written  in 
FORTRAN  to  insure  transportability,  and  they  have  been  tested  in  the  NBS  Automation 
Laboratory  prior  to  release  to  ORNL.    The  proposed  modifications  will  result  in  at  least 
a  ten-fold  increase  in  the  rate  of  task  completion  for  the  remote  manipulators  with  a 
substantial  reduction  in  fatigue  to  the  manipulator  operator. 
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II.    OPERATIONAL  REQUIREMENTS 


As  a  result  of  discussions  with  ORNL  the  following  operational  requirements  for 
HTGR  fuel  refabrication  were  transmitted  to  NBS. 

The  manipulator  systems  servicing  Cells  C,  D,  and  E  of  TURF  are  PaR  Model  3000 
electromechanical  manipulators  on  overhead  bridges.    The  standard  controls  are  rate  control 
switches  for  each  separate  joint  on  a  control  box  operated  from  outside  of  the  cell;  if 
necessary,  TV  viewing  is  used  to  assist  the  operator. 

The  TURF  facility  already  had  the  PaR  manipulators  installed  when  this  project  began. 
The  concepts  presented  here  are  directly  focused  for  that  facility,  as  directed  by  the 
ERDA/NBS  agreement.    However,  the  computer  control  concepts  discussed  in  this  report  are 
generally  applicable  to  any  remote  control  electromechanical  manipulator  and  should 
this  benefit  other  ERDA  facilities  and  to  other  Government  programs. 

Significant  improvements  in  the  operation  of  such  remote  manipulators  are  possible 
through  the  addition  of  more  advanced  master-slave  control  systems.    For  example, 
Whitney  and  Nevins  at  the  Charles  Stark  Draper  Laboratory  document  a  full  order  of 
magnitude  improvement  in  task  completion  time  with  master-slave  control  systems  compared 
with  switch  box  control  systems. 

Operation  of  the  High  Temperature  Gas-Colled  Reactor  (HTGR)  Fuel  Refabrication 
Pilot  Plant  (FRPP)  includes  such  operations  as  refilling  feeder  bowls,  moving  assembled 
fuel  elements  to  annealing  furnaces,  and  changing  furnace  liners.    ORNL  personnel  have 
indicated  that  these  operations  will  be  performed  automatically  by  the  manipulator  system 
if  automation  demonstrates  its  superiority  in  rate  of  task  completion. 

In  addition,  the  manipulator  system  will  be  required  to  perform  remote  maintenance 
operations  for  the  fuel  reprocessing  equipment,  Including  coupling  disconnects,  nonroutine 
high  dexterity  operations,  and  movement  of  major  pieces  of  processing  equipment  to  Cell  B 
for  repair.    These  complex,  non-repetitive  tasks  will  require  a  manual  master/slave  control 
mode. 

The  manipulator  control  concepts  required  to  carry  out  those  operations  are  considered 
next.    There  are  two  key  elements  in  the  proposed  facility  modifications:    The  addition 
of  position  servos  to  the  manipulator  joints  to  allow  master/slave  control  and  the 
addition  of  a  small  control  computer  to  allow  automatic  control.    The  computer  will 
be  able  to  provide  robot  operation  of  the  manipulator  and  automatic  traversing  and 
positioning  of  the  overhead  crane;  it  will  also  provide  control  of  remote  TV  cameras 
and  monitoring  of  total  system  operation  for  failure  and  for  collision  avoidance. 
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III.    CONTROL  REQUIREMENTS 


The  problem  of  controlling  a  manipulator  system  can  be  thought  as  a  continuous 
spectrum  from  direct  rate  control  of  each  joint  (as  in  current  remote  control 
manipulator  systems)  up  to  complete  autonomous  adaptive  operation  under  computer  control. 
Greater  flexibility  and  manipulative  capability  is  gained  by  moving  along  this  spectrum, 
augmenting  man's  ability  to  operate  the  remote  manipulators  through  the  addition  of 
servo  and  computer  controls. 

A  phased  approach  (recommended  by  ORNL  and  endorsed  by  NBS)  to  improve  the  operation 
of  the  manipulators  for  the  HTGR  FRPP  is  as  follows: 

Step  1 :     ADD  POSITION  SERVOS 

Step  2:      ADD  MASTER/SLAVE  CONTROL 

Step  3:     ADD  COMPUTER  INTERFACE 

Step  4:      ADD  COMPUTER  SOFTWARE  FOR  ROBOT  CONTROL 

These  steps  are  now  considered  in  detail. 

Step  1:    ADD  POSITION  SERVOS 

Adding  position  servos  to  each  joint  of  the  manipulator  is  one  of  the  most  expensive 
and  difficult  steps,  but  it  is  absolutely  essential  to  any  additional  improvements. 

Position  Sensors 

Position  servos  require  sensors  that  will  measure  the  position  of  each  joint. 
Candidates  sensors  are  potentiometers,  encoders,  and  resolvers.    The  criteria  for  selection 
are  reliability,  wires  per  joint,  and  noise  inriunity. 

Potentiometers  are  the  least  expensive  sensor,  but  they  provide  an  output  that  will  be 
susceptible  to  noise.    In  addition,  voltage  drops  over  the  extensive  cables  required  to 
connect  the  potentiometers  with  the  controls  external  to  the  cell  will  appear  as  a  position 
dependent  error.    Potentiometers  require  one  lead  (plus  a  power  supply  and  a  common 
ground)  per  joint. 

Encoders  may  be  either  incremental  or  absolute.    Incremental  encoders  give  pulses  at 
equally  spaced  angular  increments.    Absolute  position  is  formed  by  summing  the  pulses  in 
an  up-down  counter.    Incremental  encoders  require  two  wires  per  joint  for  the  two  pulse 
trains  required  to  determine  the  direction  of  motion  (plus  a  common  ground  and  common  power 
supply  lead),  but  they  can  be  susceptible  to  pulse  noise  or  power  failure.    The  number  of 
electric  motors  (noise  sources)  present  on  the  manipulators  and  in  the  FRPP  equipment  dictate 
against  incremental  encoders  if  automatic  operation  is  desired.    Absolute  encoders  have 
good  noise  immunity,  but  require  1  lead  per  bit.    This  means  that  12  bit  encoders  need 
either  12  wires  per  joint  or  a  multiplexer  in  the  radioactive  environment. 

Resolvers  offer  the  best  alternative.    Resolvers  are  extremely  reliable,  and  give  good 
noise  immunity.  They  require  three  leads  per  joint  (plus  a  common  ground). 

Velocity  Sensors 

The  manipulator  arm  positioning  tolerance  defined  by  Oak  Ridge  requires  a  position 
resolution  of  one  part  in  ten-thousand.    This  high  resolution  requires  such  a  high 
position  feedback  gain  that  compensating  velocity  feedback  is  required  to  achieve  a 
stable  position  servo.    Commercial  dc  servo  motors  are  available  which  incorporate  a 
dc  motor  tachometer  to  provide  the  required  velocity  feedback  voltage. 

Position  Servos 

Once  position  resolvers  and  dc  servo  motors  with  tachometers  are  mounted  on  the 
joints  of  the  manipulator,  the  control  loop  can  be  established  by  feeding  velocity  and 
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position  voltages  into  a  single  operational  amplifier.  The  output  of  the  amplifier 
can  then  directly  drive  a  power  amplifier  which  in  turn  drives  a  dc  servo  motor,  as 
shown  in  Figure  1 . 


Power 


Position  Command 


Amp! if ier 


(e.g.  voltage  from 
potentiometer  on 
replica  master  joint) 


To  servo 
Motor  on 
PaR  Manipulator 


Position  Feedback  (e.g.  voltage  from  resolver) 


Velocity  Feedback  (e.g.  voltage  from  tachometer)  - 


Figure  1    Servo  Schematic 


Step  2:    ADD  MASTER/SLAVE  CONTROL 

Once  a  closed  loop  control  has  been  added,  a  replica  master  can  be  for  control; 
ORNL  already  has  such  a  master  control  with  articulations  matching  the  existing  manipulator 
The  outputs  of  potentiometers  on  each  master  joint  provide  the  position  commands  for 
the  manipulator  position  servos.    The  operator  moves  the  hand  of  the  master  in  a  desired  motion, 
and  a  PaR  3000  is  servoed  to  follow  that  motion.    This  should  result  in  increased  operating 
speed  by  up  to  an  order  of  magnitude  by  allowing  the  operator  to  simulataneously  operate 
and  coordinate  the  motions  of  all  of  the  joints  to  achieve  the  desired  hand  motions. 
This  addition  is  considered  essential  to  expedite  FRPP  maintenance  operations. 

Step  3:    ADD  COMPUTER  INTERFACE 

The  addition  of  a  small  computer  to  the  control  system  allows  a  further  increase  in 
operational  efficiency.    The  computer  and  interface  circuitry  is  a  major  expense,  but  one 
that  will  result  in  clear  cost  benefits  in  allowing  automatic  operation  of  the  manipulator. 
In  particular,  once  the  proper  trajectories  are  recorded,  the  computer  can  move  the 
manipulator  around  the  cells  within  the  accuracy  of  the  servos  and  at  maximum  velocity 
without  any  danger  of  collision  with  FRPP  equipment. 


Computer  Interface 

The  design  of  a  computer  interface  depends  on  the  specific  computer  hardware 
configuration  and  its  location  with  respect  to  the  manipulator. 

The  interface  design  used  by  NBS  is  described  in  Appendix  C.  The  computer  is 
approximately  100  feet  from  the  manipulator.    The  interface  consists  of: 


Input 


1) 


A  commercial  A/D  converter  consisting  of  a  64  channel  multiplexer  rated 
at  20KH2  word  rate  with  single-ended  inputs. 


2) 


A  16  bit  parallel  connection  to  a  DR-llC  general  purpose  interface  card 
on  the  PDP-11  control  computer. 
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Output 

3)  A  16  bit  parallel  connection  from  the  DR-llC  to  the  D/A  circuitry. 

4)  A  16  channel  D/A  output  15  bit,  sequential  addressing  only.    16th  bit 
is  for  reset. 

The  interface  system,  utilizing  a  single  16  bit  parallel  I/O  interface,  0/C  does 
not  use  interrupts,  all  I/O  is  handled  on  a  polled  basis.    Therefore,  this  system 
design  works  well  with  any  Operating  System  capable  of  supporting  a  "stand-alone"  program. 
All  inputs  are  read  into  the  input  buffer;  and   control  calculations  are  made  using 
conmands  which  are  loaded  into  an  output  buffer  and  then  transmitted  to  the  dedicated 
D/A  converters  for  each  joint  servo.    See  Figure  2. 

The  important  elements  of  an  interface  design  recommended  for  the  HTGR  FRPP  facility 

are: 

1)  Use  of  single  channel  input  and  output. 

2)  Use  of  minimum  interrupt  structures  except  for  safety  interlocks.    The  use  of  a 
a  real-time  operating  system  that  is  interrupt  driven  will  require  use  of 
Interrupts  for  input/output  control. 

3)  Use  of  commercial  D/A  and  A/D  units  that  will  be  more  reliable  and  less  expensive 
than  in-house  designs; 

4)  Use  of  a  control  panel  with  several  off/on  switches  and  a  potentiometer.  (This 
control  panel  has  proven  to  be  very  useful  in  programming  and  controlling 
computer  controlled  manipulators.) 

Bringing  up  a  reliable,  noise-free  interface  1s  often  an  extensive  engineering 
effort.    Simplicity  of  design  is  very  important.    The  test  programs  in  Appendix  D  have 
proven  helpful  in  debugging  interface  and  conmunlcations  circuits. 

Step  4:  ADD  COMPUTER  SOFTWARE  FOR  ROBOT  CONTROL 

The  software  for  automatic  (robot)  operation  of  the  manipulator  system  is 
straightforward.    NBS  has  followed  a  hierarchical  structure  to  software  development 
consisting  of  5  levels: 

Level  1.    Interface  Drivers:    Interface  drivers  are  programs  written  in 

assembly  language.    They  Isolate  the  computer  dependent  I/O  from 
the  higher  level  software  programs. 

Level  2.    Trajectory  Control :    Trajectory  control  of  the  robot,  accomplished 
with  a  record-and-pl ay-back  technique,  allows  the  robot  to  do 
both  point-to-point  and  continuous  path  trajectories.  Also, 
coordinate  transformations  of  recorded  trajectories  are  accomplished 
under  program  control . 

Level  3.    Elemental  Moves:    Elemental  moves  such  as  "docking"  are  accomplished 
using  sensory  feedback  to  allow  adaptive  control  in  a 
partially  unstructured  environment. 

Level  4.    Simple  Tasks:    Simple  tasks,  such  as  "replace  furnace  liners",  are 
sequences  of  elemental  moves. 

Level  5.  Complex  Tasks:  Complex  tasks  are  the  highest  level  control  programs, 
such  as  "move  broken  index  table  to  Cell  B  for  repair".  This  program 
level  calls  up  sequences  of  lower  level  programs. 
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FIGURE  2.  LABORATORY  INTERFACE  SCHEMATIC 
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Most  of  the  operations  required  of  the  manipulators  for  the  FRPP  can  be  carried 
out  by  implementing  Level  1  and  Level  2.    In  particular,  implementing  the  continuous  path 
and  point-to-point  trajectory  record  and  play  back  at  Level  2  allows  execution  of  the 
automatic  operations  required.    These  programs  are  described  and  listed  in  Appendicies 
B,  D,  and  E. 

For  initial  experiments  it  is  recommended  that  only  interface  drivers  and  trajectory 
control  programs  be  implemented.    Testing  for  accuracy,  repeatability,  and  stability  of  the 
servos  and  computer  controls  can  be  carried  out  with  this  level  of  control. 

Coordinate  transformations  are  useful  in  conjunction  with  sensor  inputs  for  Level  3. 
Coordinate  transformation  capability  will  be  needed  if  proximity,  force,  or  touch  sensors 
are  required  for  docking,  moving  fuel  elements  to  the  annealing  furnace,  inserting 
furnace  liners,  or  similar  operations.    Sensors  on  the  hand  of  the  manipulator  or  on 
external  objects  in  the  environment  of  the  manipulator  generate  data  that  is  in  the 
coordinate  system  of  the  sensors  themselves.    For  example,  force  sensors  in  the 
manipulator  wrist  will  generate  Fx,  Fy,  and  Fz  data  referred  to  the  wrist  orientation. 
Closed  loop  control  using  this  data  inplies  that  motions  of  the  manipulator  must  be  carried 
out  in  the  same  coordinate  system  as  the  sensor  data  (e.g.  inserting  a  furnace  liner  requires 
forces  orthogonal  to  the  axis  of  the  furnace  should  be  zero  and  the  manipulator  must  there- 
fore move  exactly  along  the  axis  of  the  furnace).    This  closed  loop  control  in  turn  requires 
transforming  x  y  z  motions  into  coordinated  motions  of  the  manipulator  joints.  Coordinate 
transformation  capability  can  be  added  later,  after  the  need  for  sensory  feedback  has  been 
determined. 

It  should  be  noted  that  jigging  and  compliance  can  replace  the  need  for  Level  3 
control  programs  in  some  cases.    In  particular,  robot  manufacturers  in  both  the  U.S.  and 
Japan  have  used  compliance  and  vibration  with  open  loop  positioning  for  complex 
assembly  tasks. 
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IV.    SUMMARY  AND  CONCLUSIONS 


The  operational  requirements  described  in  Part  II  may  be  achieved  by  implementing 
the  following  steps: 

Step  1:     Add  resolvers  and  tachometers  to  each  joint  of  the  manipulator. 
Then  close  servo  loops  with  position  and  velocity  feedback. 

Step  2:      Add  master/slave  control  with  a  replica  master  using  position  transducers 
to  drive  the  manipulator  position  servo  loops. 

Step  3:     Add  mi ni -computer  with  appropriate  interfaces. 

Step  4:      Implement  robot  control  programs  for  point-to-point  and  continuous 
trajectory  control . 

Steps  1  and  2  together  give  the  capability  of  master/slave  control  for  maintenance 
and  repair  and  other  non-repetitive  operations.  Steps  3  and  4  allows  automatic  (robot) 
operations  for  routine  repetitive  tasks. 

Testing  of  these  modifications  should  be  carried  out  before  additional  change  are  made. 
The  results  of  testing  Steps  1  through  4  will  determine  the  need  for  force,  touch  or 
proximity  sensors  and  Level  3  control  programs  to  carry  out  routine  operations  such  as 
docking,  filling  feeder  bowls,  and  replacing  furnace  liners. 

Implementation  of  these  proposed  modifications  will  result  in  increased  speed  of 
operation  of  the  manipulator  and  overhead  bridge  by  at  least  a  factor  of  10  and  will 
allow  routine  transfer  and  service  operations  to  be  accomplished  automatically. 
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Appendix  A: 

Letter  from  ERDA  (then  AEC)  Oak  Ridge  Operations  Office  to  NBS,  November  13,  1974, 
requesting  a  conceptual  design  study  of  a  programmable  control  system  for  an  electro- 
mechanical manipulator  system. 
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Appendix  B:    DESCRIPTION  OF  NBS  COMPUTER  CONTROL  SOFTWARE 


1.  Introduction 

Recently  a  series  of  experiments  was  conducted  In  the  ICST  Automeition  Technology 
Laboratory  in  which  the  computer  control  of  a  mechanical  manipulator  was  studied.    In  this 
experiment,  a  manipulator  was  interfaced  to  a  mini-computer. 

The  lowest  levels  of  computer  software  for  controlling  this  manipulator  as  a 
robot  are  described  in  this  report.    The  discussion  will  describe  software  strategy 
and  will  examine  the  general  problem  of  specifying  a  mini-computer  for  program  develop- 
ment and  real  time  control. 

The  system  described  is  a  PDP-11  computer  with  D/A  and  A/D  interfaces,  a 
hydraulic  power  anthropomorphic  manipulator  system  with  full  hardware  servos,  and  an 
exoskeleton  harness  control  for  master/slave  operation.    The  techniques  discussed 
are  generally  applicable  to  the  computer  control  of  manipulators. 

a.  Problem  Statement 

While  the  main  function  of  the  laboratory  mini -computer  Is  to  execute 
the  manipulator  control  programs,  program  preparation,  compilation,  linking  and  loading 
must  also  be  performed.    In  general,  a  dedicated  laboratory  mini-computer  must  have 
more  than  just  a  central  processing  unit  (CPU)  and  a  program  store  (core)  to  carry 
out  these  program  development  steps.    The  issues  of  what  is  required  in  the  mini-computer 
and  its  associated  hardware  are  discussed  as  they  relate  to  the  problem  of  sizing  a 
machine  (the  mini-computer)  to  do  the  necessary  control  functions.    First,  a  description 
of  the  software  system  developed  in  the  ICST  laboratory  is  discussed.    Then,  a 
description  of  the  necessary  elements  in  the  mini-computer  architecture  as  well  as  a 
general  description  of  a  possible  mini-computer  configuration  is  presented. 

b.  System  Overview 

The  laboratory  mini-computer  is  the  central  coordinating  point  for  all  of 
the  manipulator  hardware.    A  16  bit  general  purpose  parallel  interface  connects  the 
computer  to  the  manipulator.    The  interface  output  buffer  is  connected  to  a  D/A 
converter  designed  by  NBS.    The  16  bitoarallel  output  is  switched  sequentially  to 
buffer  registers.    These  buffer  registers  drive  digital  to  analog  (D/A)  converters  which 
drive  the  servo  valves  on  the  joints  of  the  manipulator.    There  is  one  D/A  converter 
for  each  joint  of  the  manipulator. 

Input  to  the  computer  from  the  manipulator  hardware  is  from  a  64  channel 
analog  to  digital  (A/D)  multiplexer  and  converter.    The  signals  to  the  multiplexer 
are  derived  from  potentiometers  and  other  sensors  connected  to  the  manipulator,  the 
exoskeleton,  and  a  special  panel  containing  function  switches  and  potentiometers. 
The  A/D  converter  is  connected  to  the  input  buffer  of  the  general  purpose  interface 
board. 

In  the  system  described  here,  several  software  programs  were  prepared 
and  executed  on  the  mini-computer.    The  programs  interact  with  the  operator  through 
the  operators's  console  which  also  provides  system  status  information.    The  system 
software  was  developed  in  a  modular  fashion  so  that  new  applications  could  easily 
be  built  by  incorporating  existing  software. 

2.  Software  System 

The  ICST  software  system  for  mini -computer  control  appears  as  a  hierarchy 
of  modular  control  functions  easily  organized  into  a  comprehensive  applications  package 
for  controlling  a  manipulator.    Three  hierarchical  levels  of  software  are  defined. 
At  the  lowest  level  are  assembly  language  input  and  output  drivers  for  the  interface 
to  the  manipulator.    At  the  next  level  are  programs  for  teaching  the  manipulator  to 
carry  out  various  trajectories.    At  the  highest  level  are  programs  for  coordinate 
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transformations  which  allow  trajectories  to  be  defined  in  an  experimental  (laboratory) 
coordinate  system.    The  first  two  levels  are  discussed  here. 

Appropriate  sensor  input  to  predefined  A/D  converter  channels  enable  the  mini- 
computer to  control  the  manipulator  in  response  to  the  sensory  stimuli.  Acoustic 
positioning  feedback  sensors,  force  sensors  and  pressure  sensors  have  already  been 
used  in  the  laboratory  experiments.    The  hierarchy  of  software  programs  used  to  provide 
the  control  functions  is  described. 

a.  Hierarchy 

1 .  Lowest  Level 

At  this  level,  two  driver  modules  are  specified.    ARMIN  is  the  driver 
which  transfers  data  from  the  A/D  multiplexer  into  a  core  buffer  and  ARMOUT  transfers 
data  from  a  core  buffer  to  D/A  converters  which  are  connected  to  the  servo  valves 
which  control  the  manipulator  joint  positions. 

2.  Second  Level 

The  first  application  program  in  the  system  is  called  SANDF. 
SANDF  is  a  mnemonic  for  "store  and  forward".    The  program  uses  the  driver  program 
ARMIN  to  read  the  input  channels.    Included  in  the  input  channel  data  are  the  positions 
of  the  exoskeleton  joint  angles.    These  exoskeleton  channel  datum  are  scaled  by  SANDF 
after  which  SANDF  calls  the  driver  ARMOUT.    ARMOUT  writes  the  scaled  exoskeleton  posi- 
tions to  the  servo  valves  of  the  manipulator.    In  this  way,  SANDF  is  able  to  control 
the  manipulator  by  causing  it  to  track  the  exoskeleton.    Successive  calls  to  SANDF 
will  cause  the  manipulator  to  follow  the  trajectory  of  the  exoskeleton. 

Building  upon  this  existing  software,  an  application  program  called 
ARM  was  designed  to  behave  much  like  a  tape  recorder.    In  the  ARM  program,  the  exo- 
skeleton positions  are  stored;  then  forwarded  to  the  manipulator  with  the  SANDF 
program.    At  the  operator's  request,  the  manipulator  coordinates  are  recorded  by  the 
program  and  stored  in  the  mini-computer.    Much  like  a  tape  recorder,  the  operator  can 
play  back  these  coordinates  and  cause  the  manipulator  to  traverse  the  pre-recorded 
trajectory.    ARM  allows  us  to  operate  at  the  sophistication  level  of  commercially 
available  industrial  robots. 

Embellishments  to  this  program  are  worth  noting.    By  utilizing  the 
file  system  of  the  mini-computer,  sets  of  coordinates  can  be  saved,  and  later  retrieved 
by  name  for  play  back.    Also,  during  play  back,  a  special  function  button  panel  can 
be  accessed.    This  panel  has  a  potentiometer  that  can  be  manually  set  by  the  operator. 
The  program  ARM  uses  the  potentiometer  setting  to  indicate  the  speed  at  which  the  play 
back  is  to  occur.    While  the  maximum  play  back  is  limited  by  the  execution  time  for 
the  program,  delays  in  play  back  are  introduced  by  reference  to  a  routine  called 
TIMOUT  which  accepts,  as  an  argument,  a  delay  specified  in  60ths  of  a  second.  The 
potentiometer  value  is  approprimately  scaled  to  indicate  a  suitable  delay.  Full 
speed  play  back  is  significantly  faster  than  the  recording  speed  so  that  the  play 
back  speed  ranges  from  almost  twice  as  fast  as  normal,  to  very  very  slow  —  where  each 
recorded  point  in  the  trajectory  is  easily  noticeable. 

A  second  program,  PNTPNT,  allows  point-to-point  trajectories  to  be 
recorded  and  executed.    Points  are  recorded  when  the  carriage  return  is  typed  on  the 
control  terminal  rather  than  at  equally  spaced  time  intervals. 

A  third  program,  NEWARM,  combines  these  two  programs  and  adds  interpola- 
tion between  stored  points  along  the  trajectory  during  play  back. 


3.  Software  Requirements 

While  the  main  function  of  the  mini-computer  is  to  execute  the  control  programs 
for  the  manipulator,  program  preparation,  compilation,  linking  and  loading  must  also 
be  performed.    The  basic  operating  system  usually  supplied  with  the  mini -computer 
should  provide  these  functions. 

a.  Program  Preparation 

Once  conceived  by  the  programmer,  the  program  must  be  entered  into  the 
computer  system.    Typically,  the  mini -computer  is  supplied  with  a  keyboard  control 
terminal  and  an  operating  system  containing  an  editing  system  for  entering  new  programs 
or  modifying  existing  ones.    If  the  mini-computer  has  a  disc  based  operating  system 
(DOS),  the  program  source  files  are  easily  maintained  as  text  files  on  the  system 
disc.    DOS  can  also  be  used  to  support  the  compilers  and/or  assemblers  used  to  trans- 
form the  source  language  statements  into  machine  instructions.    If  no  disc  is  available, 
some  other  means  of  storing  the  source  program  and  compilers  is  required.  Usually 
this  is  done  with  paper  tape.    Such  systems,  while  less  expensive  to  procure,  generally 
require  significant  amounts  of  time  just  to  do  simple  operations  like  compiling  programs. 
Thus,  the  initial  cost  savings  of  a  paper  tape  system  are  quickly  absorbed  in  non-productive 
labor  costs. 

b.  Program  Compilation  (Assembly) 

Once  the  programmer  is  satisfied  that  the  text  of  his  source  file  is 
correct,  he  can  compile  the  program;  if  the  text  of  his  source  file  is  an  assembly 
language  program,  he  can  assemble  his  program.    Higher  level  language  programs,  which 
require  compilation,  are  usually  easier  to  read,  easier  to  write,  and  easier  to  debug; 
but,  inefficiencies  in  machine  code  are  generally  introduced.    Many  systems  will 
therefore,  allow  the  main  programs  and  many  subroutines  to  be  written  in  a  higher  level 
language  which  is  compiled,  and  allow  the  routines  which  require  extreme  efficiency 
to  be  coded  in  assembly  language.    In  order  to  put  together  the  compiled  programs 
and  the  assembly  language  programs  in  preparation  for  executing  the  system,  a  processes 
called  linking  is  performed. 

c.  Program  Linking  and  Loading 

To  link  programs  together  into  a  complete  system  requires  a  special  operat- 
ing system  routine  called  a  "Linker".    This  program  assigns  core  locations  to  the 
variables  defined  in  the  user  programs  and  assigns  space  in  main  memory  for  the  actual 
programs  to  be  executed.    The  output  of  the  linker  program  is  typically  a  file  containing 
a  core  image  of  the  linked  program,  or  a  file  containing  an  image  of  the  linked  program 
along  with  special  loader  instructions  specifying  how  the  linked  program  is  to  be 
loaded.    In  any  case,  once  the  program  is  linked,  a  loader  is  capable  of  preparing 
a  core  image  which  can  be  executed  by  the  CPU. 

d.  Program  Execution 

Once  the  loader  is  finished  preparing  a  core  image  of  the  linked  program, 
the  loader  transfers  control  of  the  CPU  to  the  start  address  of  the  loaded  program. 

4.  Hardware  Configuration 

The  hardware  configuration  for  a  mini -computer  used  to  support  program  prepara- 
tion is  often  significantly  different  from  that  of  a  mini-computer  used  to  execute 
a  dedicated  user  program.    In  one  case  a  disc  based  operating  system  used  to  edit, 
compile,  assemble,  link  and  load,  might  require  a  high  speed  line  printer  for  listings 
of  source  programs,  a  disc  pack  peripheral  for  mounting  user  program  libraries  of 
commonly  used  debugging  aids  and  much  more.    The  dedicated  user  program  on  the  other 
hand  will  only  require  a  few  peripherals  specific  to  the  needs  of  the  intended  applica- 
tion, for  example,  an  interface  to  a  manipulator. 
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a.    Program  Preparation 


Experience  indicates  that  in  order  to  do  program  preparation  on  a  mini- 
computer a  disc  based  operating  system  is  prefered  to  a  paper  tape  system. 
Also  at  least  some  hard  copy  printing  device  is  necessary  to  get  listings  of  the  pro- 
grams written  as  well  as  any  data  the  program  may  generate.    While  a  high  speed  line 
printer  is  necessary  on  a  machine  dedicated  to  program  development,  the  high  cost 
is  not  justified  for  intermittent  program  development  sessions.    Finally,  a  mountable 
secondary  storage  medium  such  as  magnetic  tapes  or  disc  packs  is  necessary  when 
installing  the  manufacture's  operating  system.    Mountable  storage  is  also  very  useful 
whenever  large  volumes  of  data  are  collected  by  the  application  or  user  program. 

b.    Program  Execution 

The  mini-computer  configuration  requirements  for  program  execution  are 
much  different  than  for  program  perparation.    The  only  peripheral  devices  required  are  those 
which  are  accessed  by  the  program.    The  normal  peripherals  used  in  program  preparation 
however,  can  often  serve  dual  purposes.  The  operator's  console  can  be  used  to  list 
information  regarding  the  status  of  the  user  program.    Secondary  storage  is  also  used 
if  the  user  program  generates  large  volumes  of  data.    Also,  in  systems  where  core 
size  is  limited,  provisions  are  often  made  to  overlay  portions  of  the  user  program. 
This  technique  allows  programs  to  be  written  which  use  more  core  than  is  available 
at  any  one  time.    The  user  program  is  organized  in  blocks  or  segments  which  can  easily 
be  swapped  between  core  storage  and  disc  storage.  While  this  technique  usually 
slows  down  the  execution  of  the  user  program,  the  benefits  gained  in  available  program 
size  are  usually  justified. 

The  special  peripheral  requirements  used  in  the  manipulator  control 
programs  include  the  parallel  interface  to  the  A/D  and  D/A  converter  systems.  The 
special  function  button  panel  connected  to  the  input  A/D  multiplexer  provides  an 
excellent  means  to  control  the  manipulator  software.    Also,  the  visual  operator 
feedback  from  the  blinking  lights  on  the  multiplexer  provides  assurances  to  the  operator 
that  the  mini-computer  is  functioning  properly.    This  has  proved  to  be  a  valuable  aid 
during  the  debugging  phase  of  the  software  system. 

5.  Conclusions 

The  intent  of  this  report  is  to  focus  on  the  computer  power  and  software 
techniques  required  to  control  a  complex  mechanical  manipulator.    Experiments  in  the 
ICST  Automation  Technology  Laboratory  indicate  that  sophisticated  control  functions 
can  be  performed  using  a  mini-computer  equipped  with  the  proper  peripherals  and  equipped 
with  the  proper  software  tools  to  develop  the  application  programs. 

In  the  ICST  Laboratory,  a  DEC  PDP-11/20  was  originally  used  to  control  the 
manipulator.    In  preparing  programs,  a  disc  operating  system  equipped  with  a  program 
editor,  a  FORTRAN  compiler,  a  macro  assembler  and  a  linking  loader  were  used.  Control 
programs  were  later  transferred  to  a  PDP  11/45  again  using  DOS.    All  software  described 
here  could  be  prepared  and  run  on  a  PDP  11/20  size  mini -computer. 

Hardware  peripherals  unique  to  the  requirements  of  the  laboratory  were 
utilized  by  the  applications  programs.    A  special  function  button  panel  connected  to 
several  input  channels  of  the  A/D  converter  proved  to  be  a  very  useful  and  desirable 
operator  convenience  in  controlling  which  software  module  was  to  be  executed.  Also, 
the  visual  feedback  from  the  blinking  lights  on  the  A/D  converter  reassured  the 
operator  that  the  programming  system  was  operational. 
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Appendix  C:    INTERFACE  DESIGN 


Data  transfer  between  the  laboratory  hardware  and  the  PDP-n/45  computer  is 
handled  via  a  DRll-C  interface  card  in  the  computer.    This  card  has  16  data  input  lines, 
16  data  output  lines,  two  interrupt  lines  and  two  sync  lines,  one  (data  transmitted)  for 
input  synchronization,  and  one  (data  present)  for  output  synchronization. 

The  DRll-C  is  connected  to  the  laboratory  interface  by  forty  parallel  wires  powered 
by  7406  line  drivers.    These  are  open  collector  circuits  with  resistors  connected  to 
the  power  supply  voltage  at  the  receiving  end. 

The  laboratory  interface  hardware  consists  of  two  parts:  an  input  section  and  an 
output  section. 


Input: 

A  sixty  four  channel  analog  multiplexer  followed  by  a  14  bit  analog  to  digital 
converter  forms  the  heart  of  the  input  section.    This  is  range  -10  volts  to  +10  volts. 
Negative  voltages  are  converted  to  2's  complement  form. 

Output  from  the  A/D  converter  is  carried  to  the  computer  on  the  16  data  input  lines. 
Since  the  A/D  converter  has  only  a  14  bit  output,  the  most  significant  (sign)  bit  is 
connected  to  the  three  most  significant  computer  input  lines.    This  causes  the  computer 
to  read  the  correct  values  for  both  positive  and  negative  (2's  complement)  numbers. 

Data  transfer  between  the  A/D  converter  and  the  computer  is  signaled  by  a  pulse 
on  the  data  transmitted  line.    This  pulse,  initiated  by  the  computer,  indicates  that  the 
data  lines  have  been  sampled.    This  data  transmitted  pulse  is  used  to  step  the  analog 
multiplexer  to  the  next  input  and  to  initiate  the  analog  to  digital  conversion  cycle  on 
the  new  input  voltage.    The  input  software  is  timed  to  sample  the  input  data  at  a  rate 
slower  than  the  conversion  rate  of  the  A/D  system.    Thus,  a  "conversion  completed"  signed 
is  not  required  by  the  computer,  and  only  one  control  line  is  needed  to  advance 
the  converter  to  the  next  channel . 

The  multiplexer  is  operated  in  a  sequential  mode.    It  is  reset  to  the  zeroth  channel 
by  a  reset  pulse  derived  from  the  output  section.    Each  data  transmitted  pulse  steps  the 
multiplexer  to  the  next  sequential  channel.    The  input  section    is  this  completely  under 
computer  control . 


Output: 

A  sixteen  channel  digital  demultiplexer  routes  output  signals  from  the  DRll-C  to 
sixteen  holdings  registers.    These  provide  either  digital  output  directly,  or  are  connected 
to  digital  to  analog  convert  so  as  to  provide  analog  voltabes.    Datatransfer  between  the 
DRll-C  and  the  output  section  is  initiated  by  a  pulse  on  the  data  present  line.    The  data 
present  pulse  occurs  simultaneously  with  the  appearance  of  data  on  the  DRll-C  output  lines 
This  pulse  is  delayed  in  the  laboratory  interface  hardware  for  two  microseconds  before 
actuating  the  digital  demultiplexer  so  as  to  allow  the  data  lines  time  to  settle.  The 
delayed  data  present  pulse  gates  the  output  data  into  the  selected  holding  register  and  then 
steps  the  address  counter  to  the  next  address. 

The  most  significant  (sign)  bit  of  the  output  is  decoded  as  a  reset  signal  and  is 
used  to  set  the  demultiplexer  address  counter  to  Channel  1  as  well  as  to  reset  the  analog 
multiplexer  address  in  the  input  section. 

Using  the  sign  bit  for  control  means  that  only  15  bits  of  output  are  available  for 
data.    Numerical  is  restricted  to  the  range  0  to  32,767.    The  digital  to  analog  converts 
are  adjusted  such  that  numerical  0  corresponds  to  +10  volts;  16,384  corresponds  to  zero 
volts,  and  32,767  corresponds  to  -10  volts. 
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Appendix  D:    INTERFACE  DRIVERS  AND  TEST  SOFTWARE 


The  following  programs  are  included  in  this  Appendix: 

1.  ARMIN:      An  Assembly  Language  input  driver  for  the  DR-llC  Interface 

2.  ARMOUT:     An  Assembly  Language  output  driver  for  the  DR-llC  interface 

3.  TEST:       A  FORTRAN  program  for  displaying  and  testing  the  values  of  the  A/D 

input  circuits. 

4.  SWTH:       A  staircase  function  generator  for  testing  the  D/A  output  circuits  by 

generating  a  sawtooth  wave  form. 

5.  INTFC:      Interface  tester  for  testing  loop  from  D/A  through  A/D  functions 

which  is  looped  back  to  input  and  displayed.    Also  displays 
neighboring  channels  to  test  crosstalk. 

6.  SAAR:       Sine  wave  generator  with  step  size  limiting  for  safety.  Exercises 

all  joints  at  once  for  servo  and  total  system  testing. 

Notes:  These  programs  are  written  in  FORTRAN  and  operate  on  PDP-n/45 

under  DOS.    They  should  also  operate  successfully  on  small ier  members 
of  the  PDP-11  family.    Operation  of  these  programs  under  RSX,  UNIX,  ELF 
or  other  operating  systems,  or  operation  of  other  mini -computers  will 
require  rewriting  ARMIN  and  ARMOUT  and  review  of  the  I/O  calls  1n  the' 
programs. 
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.TITLE  AKMIN  FOH  LOW  LEVEL  AkM  INPUT 
.  lOEiMT  /KMRk)!/ 


AKMIN  AND  AkMOUT  HAVE  SIMILAR  DOCUMENTATION.      THE  TWO  PROGRAMS 

Should  be  read  together  as  some  documentation  exists  in 
one  and  not  the  other. 

.CSECT 

THE  ARh   IS  CONTROLLED  liY  A  DRll-C   INTERFACE   TO  THE  PDP-11/45 
UNIbUo.     ALTHOUGH  ALL  THE   INTEkRUPT   FACILITY  AVAILABLE  WITH 
THE   DRll-C   IS    INTACT,    NO   INTERRUPTS   ARE   USED  WITH  THIS  VERSION 
OF   THE   ARM  SOFTWARE.      THE  GENERAL   PHILOSOPHY   OF  THE  SOFTWARE 
IS   SUCH   THAT   DATA  TRANSFER  TO  AND   FROM  THE  ARM  WILL  OCCURE 
aHENLVER  a  PROGRAM  WANTS   AND/OR  HAS   TriE  TIME. 

THE   uRll-C   HAS  A   UNIBUS  ADDRESS   OF  16777^ 
THIS   IS  THE  CONTkOL  REGISTER.    1677  72   IS  THE  TRANSMIT  REGISTER 
AND  167774  IS  THE  RECEIVE  REGISTER.     THE  A/D  CONVERTER 

IS  RESET  WHENEVER  IT  SEES  BIT  15  SET.  AT  LOCATION  24,  THE  A/D  CONVERTER 
IS  RtSET. 

ALL  6  4  CHANNELS  ARE  READ  INTO  THE  COMMON  BLOCK  NAMED  ARMBUF 
THIS  IS  FORTRAN  COMPATABLE  COMMON.  ALSO  THE  SUBROUTINE  LINKAGE 
IS  COMPATABLE. 


VAL; 


ARMIN; 


,  WORD 


25 


1§: 


MOV 


MOV 
MOV 

MOV 
MOV 
DEC 
liPL 
MOV 
MOV 
DEC 
BGT 
RTS 


VAL  ,Rk) 


#167772, R2 
#-1, (R2)+ 

#64. ,R4 

#INBUF,R3 

1§ 

VAL,Rk) 

(R3)+ 

R4 
1$ 

R5 


TIME  OUT  VALUE 

IS  SET  AT   10   WHICH  CAUSES  THIS 
ROUTINE  TO  WAIT    (SEE   LOOP  1$) 
LONG   ENOUGH   FOR  THE  A/D  CONVERTER 
TO  SETTLE   DOwN   BETWEEN  READS. 

THE  11/4  5   RUNS  TOO  FAST 
SO  aE   will  use  R0  TO 
TIMEOUT  BETWEEN  READS. 
LOAD  THE  CSR  OUTPUT  REG 
WITH  A  RESET  AND  POINT 
TO  THE  INPUT  SIDE 
COUNT  UP  TO  64  POINTS 
POINT  TO  THE   FIRST  WORD 
COUNT  DOWN   FOR  11/45  TIMEOUT 


;LOAD  THE   INBUF  WITH   64  POINTS 


;THEN  RETURN 


IMPLICIT  INTEGER (A-Z) 

COMMON  /ARMBUF/INBUF (64) ,OUTBUF(64) 

wHt-N   USED  WITH   FORTRN ,   COMPILE  wITH  THE  /ON  SwIlCH 
•CSECT  ARMBUF 

INBUF= . 
OUTBUF=  . +2kll3 

.  =  .+4k)a 

.CSECT 
.END 
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•TITLE  ARMOUT  FOR  LOW  LtVEL  ARM  OUTPUT 
.IDENT  /RMRkJl/ 

CALLING  SEQUjiNCE  IS: 


AKMOUT 


IS: 


2$: 


CALL  ARMOUT  (COUivll  ,  [BUFFER]  ) 

WHERE  (BUFFER) 


i'lOV 
MOVii 

MOV 

I'lOV 

DEC 
dbg 

MOV 
MOV 
MOV 
DEC 
dPL 
MOV 
MOV 
DEC 

BoT 
MOV 


RTS 


IF  NOT 


,GLOBL  VAL 


VAL,Rk) 
(dR5  ,R4 

vd2(R5)  ,R3 

#0UTBUF,R2 

R4 
1$ 

4 (R5) ,R2 
#167772, R4 
#-1  ,i§R4 

Rid 
2$ 

VAL,  Rid 

(R2)+,@R4 

R3 


2$ 
#-1 


r5 


idR4 


IS  OPTIONAL 
THERE,    OUTBUF   IS  USED. 
;THE   11/45   IS   TOO  FAST,   ARMIN  AND 
; ARMOUT  MUST  SLOW  DOWN,    VAL   IS  A 
;WORD  TO  HELP  US   00  THAT. 


;GET  THE   NUriBER  OF  ARGUMENTS 
;IN  THE   CALLING  SEQUENCE 
;GET  THE  NUMBER  OF   POINTS  TO 
;3END  TO  THE  ARi"! . 
; ASSUME  OUTBUF   IS  USED 
;FOR  ARM  OUTPUT 

;THE  NUMBER  OF  ARGS   WAS   ONE  SO 
;USE  OUTBUF,  OTHERWISE 
;USE  THE  ARRAY  SPECIFIED. 
; POINT  TO  THE  ARM  INTERFACE 
;AND  RESET  IT 


11/45   IS  TOO   FASl,    SO,  TIMEOUT 

OUTPUT   FROM  THE  BUFFER 

COUNT   DOWN   ON   NUMBEk  OF  POINTS 

TO  SEND  TO  THE  AkM 

THEN   SEND  THEM 

RESET  AFTER 

THE  WRITE  (6-MAY-74) 

THIS   CAUSES   LESS  CURRENT 

DRAIN  ON  THE   POWER  SUPPLIES    IN  THE 

D/A  CONVERTER. 

THEN  GO  HOME 


Implicit  integer(a-z) 

common  /armbuf/inbuf(64) ,outbuf(64) 

.csect  armbuf 


liMiiUF=  . 
OUTouF=  .  +2id0 
.  =  .+4k3k) 

.CSECT 
.END 
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(J  ************************************* 


C  THIS   PKOGHAM  IS   USED  TO  TCST  THE  A/D  POttTION 

C  OF  THh   INTERt'ACt;.      IT  REQUESTS  AN  INPUT 

c  Channel  NUMtiER(w)  .    the  value  on  this  channel 

C  IS   PRINTED  OUT  AS  A  BINARY   NUM3ER ( dl N ( 1 5 ) ) . 

C  THIS   NUMBER  CAN   BE   COMPARED  TO  THE  BINARY 

C  NUMBER  FROM  THE  A/D  CONVERTER  DISPLAY  PANEL 

C  TO  ASCERTAIN   IF  THE  COMPUTER  IS  RECEIVING 

C  THE  CORRECT  VALUE  AND  IF   IT  IS  ACCESSING 

\  C  THE   PROPER   INPUT  CHANNEL.      IF  A  BINARY 

C  OUTPUT  ON  CHANNEL  M   IS   NOT  DESIRED,  THEN 

C  THE  VALUE  OF  M    (M=k),M<a)    IS   USED  AS  A 

C  FLAG   To    (1)    OMIT  THE  CALCULATIONS  TO 

C  CONVERT  TO  A  BINARY   NUMBER  AND    (2)  CAUSE 

C  THE  PROGRAM  TO  LOOP  CONTINUOUSLY  (AS 

C  OPPOSED  TO  STOPPING  AFTER  100  OUTPUTS) .  THE 

C  PROGRAM  ALSO  DISPLAYS  THE   DECIMAL  EQUIVALENTS 

C  OF  THE  VALUES  ON  A  SEQUENCE  OF  INPUT 

C  CHANNELS.      THE   PROGRAM,   THEREFORE,  REQUESTS 

C  Trie-  STAKilwG  CHANNEL  NUMBER(LL)  ,    AND  THE 

C  ENUIU^  CHANNbL  NUi'iBEk  ( KK)  ,   THEN  DISPLAYS 

C  THE  VALUES  ON  ALL  THE  CHANNELS   BETWEEN  THEM. 

C 

************************************* 


C 


THIS 


C 
C 

c 
c 
c 
c 

80 
198 

197 

200 

201 

C 

c 

C  THIS 

C 

\- 

c 
c 
c 

31 

10 


IMPLICIT  INTEGER  (A-Z) 

COMMON  /ARMBUF/INBUF(64) ,0UTBUF(64) 

DIMENSION   BIN (15)  , 

********** 

SECTION  OF  CODE  REQUESTS  THE  INPUT 

CHANNEL  NUMBER (M)    FOR  THE   BINARY  DISPLAY, 

AND  THE  START(LL)    AND  SrOP(KK)    FOR  THE  SERIES 

DISPLAY.      IF  ONLY   INPUT  CHANNEL  M  IS  TO 

HAVE   ITS   DECIMAL   EUIVALEnT  DISPLAYED,  THEN 

BOTH  LL  AND  KK  ARE  SET  EQUAL  TO  M. 
********** 


WRITE  (6,193) 

FORMAT    (    '      CHANNEL  NUMBER  = 
READ(6,197)  M 
FORMAT  (12) 
rtRITE(6,200) 

FORMAT (2X  STARTING  CHANNEL= 

READ(6,201)LL,KK 

FORMAT (I  3, 1  3) 


,    ENDING  CHANWEL=  ') 


********** 

'DO  LOOP'    (FROM  STATEMENT  81   TO  50) 
CAUSES  THE   PROGRAM  TO  CYCLE  THROUGH  100 
OUTPUTS   BEFORE  REQUESTING  ANOTHER  CHANNEL 
NUMBER. 
********** 

DO  50  J=l,100 
CALL  ARMIN 
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^  ********** 

C  THIS  otiCTIOW  Of  COUE  PRINTS  OUT  THE  DECIMAL 

C  EQUIVALENTS  OF  THE  VALUES   FROM  THE  A/D 

C  CONVtKTEH  FOK  THAT  SEQUENCE  OF  REQUESTED 

C  CHANNELS. 

Q  ********** 

c 

WRITE    (6,lk)tJ)    (INaUF(I)  ,I  =  LL,KK) 

10a  FORMAT  (2idX,10I5) 
C 

Q  ********** 

C  TnIS  DO  LOOP'    (TO  STATEMENT   13)  PROPERLY 

C  WEIGHTS  THE  VALUES  ON  THE   INPUT  CHANNELS 

C  AND  LOADS  THEM  IN  THE  OUTBUF  REGISTERS ( OUTBUF ( I )) . 

C  Trio  VOLTAGE  THAT  NOW  APPEARS  ON  THE  OUTPUT 

C  CHANNELS  OF  THE  D/A  CAN  3E  COMPARED  TO  THE 

C  VOLTAGE  LEVELS  ON  THE   INPUT  CHANNELS  AS  wELL 

C  AS  THE   bINARY   KEPPRESENTATION  OF  THIS  VOLTAGE 

C  IN  THE  COMPUTER  AS  A  MEASURE  OF  THE  ACCURACY 

C  OF   THE  A/D  CONVERSION   BY   ITSELF,  AND 

C  TttE   D/A  CONVERSION   BY   ITSELF.      THIS   IS  IN 

C  CONTRAST  TO  THE   INTFC   PROGRAM  WHICH  MEASURES 

C  THE   TOTAL  CONVERSION   ERROR  WITH  AND  WITHOUT 

C  THE  IIANIPULATOR  THEREBY  ALLOWING  A  MEASURE 

C  OF  THE   ERROR   IN  THE   HARDWARE  SERVOS. 

Q  ********** 

c 

DO  13  1=1,64 

13  OUTBUFd)  =2*INBUF(I) +16384 
CALL  ARrtOUT(i6) 

C 

^  ********** 

C  The  value  OF  M  IS  TESTED  HERE  TO  SEE   IF  THE 

C  bINARY  CONVERSION  SHOULD  BE  CARRIED  OUT. 

Q  ********** 

C 

IF    (M)  5k},5tJ,22 

C 

Q  ********** 

C  A   15  SIT  bINARY   NUMBER  IS  CALCULATED  FOR 

C  INPUT  CHANNEL  M  BY  THE   FOLLOWING  CODE. 

C  blN(l)    IS  THE  MOST  SIGNIFICANT  BIT.  IT 

C  IS   USED  HERE  AS  A  SIGN   BIT(0   FOR  -,    1   FOR  +)  . 

C  IF  THE   INPUT  VALUE   IS  A  NEGATIVE  NUMBER,  THEN 

C  BIN    (1)    IS  SET  EQUAL  TO  ZERO  AND  THE     DO  LOOP' 

C  THROUGH  STATEMENT  5   IS   USED  TO  DETERMINE 

C  TtlE  REMAINING  BITS.      IF  THE   INPUT  VALUE   IS  A  POSITIVE 

C  NUMBER,   THEN   BIN(l)    IS  SET  EQUAL  TO  ONE  AND  THE 

C  'DO  LOOP'   THkOUGH  STATEMENT  8   IS  USED  TO 

C  DETERMINE  THE  REMAINING  BITS. 

Q  *****^**** 

c 

22  X=8192 

N=-INBUF(M) 
IF   (N)  6,7,7 

C 

^  ********** 

C  THIS  IS  THE   BINARY  CONVERSION   FOR  A  NEGATIVE 

C  NUMBER  ON   INPUT  CHANNEL  M 

Q  ********** 

C 
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7  BIN{1)=0 

DO  5  1-1,14 
IF  (N-X)l,2,2 

1  BIN(I+1)=0 

GO  TO  5 

2  bIN(I+l)=l 
iM  =  N-X 

5  X=X/2 

GO  TO  19 

C 

Q  ********** 

C  THIS  IS- Trie;  BINARY  CONVERSION  FOR  A  POSITIVE 
C  NUi^BER  ON   INPUT  CHANNEL  M. 

Q  ********** 

6  BIN(1)=1 

1M  =  N 

DO  d  1=1,14 
IF    (N+X) 11 ,11 ,12 
12  bIN(I+l)=0 

GO  Tu  3 
11  bIN(I+l)=l 
N  =  W  +  X 

8  X=X/2 
C 

^  ********** 

C  TriE  bINARi:  EQUIVALENT  OF  THE  VALUE  OF  INPUT 
C  CHANNEL  M  IS  PRINTED  HERE. 

Q  ********** 

C  ' '" 

19  WRITE   (6,199)  BIN 

199  FORMAT  (5(1X,3I1)) 

50  CONTINUE 

IF    (M)    81  ,81  ,8k) 

END 
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Q  ************************************* 

c 

C  SWTd 


C  THIS  PROGRAM  GENERATES  A  STAIRCASE  TEST  FUNCTION. 

C  BOTH  THE  LOWER  AND  UPPER  BOUNDARIES  ARE  ENTERED 

C  IN  RESPONSE  TO  QUERIES  BY  THE  PROGRAM.     THESE  SET 

C  THE  BEGINNING  AND  ENDING  VALUES  OF  A  SIMPLE  'DO 

C  LOOP'.      THE  STEP  SIZE   IS  ALSO  ENTERED  AND  BECOMES 

C  THE   INCREMENTAL  STEP  OF  THE    'DO  LOOP'. 

C  THIS  PROGRAM  CALLS  THE  SUBROUTINES  : 

C  ARMOUT   :   SETERR  : 

C  SETERR  IS  A  FORTRAN  LIBRARY  SUBROUTINE  WITH  TWO 

C  ARGUMENTS (SETERR ( C, M) ) . 

C  C=  AN  INTEGER  INDICATING  THE  ERROR  CLASS 
C  AFFECTED.     C=3   IS  THE  VALUE  FOR  ARITHMETIC 

C  OVERFLOW.     THIS  IS  USED  IN  CASE  THE  INTEGER 

C  IN  THE    'DO  LOOP'   EXCEEDS  32,767   BEFORE  THE 

C  LOOP  IS  TERMINATED. 

C  M=  AN  INTEGER  THAT  INDICATES  DIFFERENT  INSTRUCTIONS. 
C  M=-l  TELLS  THE  SYSTEM  NOT  TO  LOG  MESSAGES,  AND 

C  TO  IGNORE  THE  ERROR  COUNT  FOR  THE  SPECIFIED 

C  ERROR  CLASS. 

C 

i--  ************************************* 


C 

INTEGER  ARMBUF, INBUF ,OUTBUF 
C0wM0n/ARMBUF/INBUF(64) ,0UTBUF(64) 
CALL  SETERR{3,-1) 

C 

Q  ********** 

C  PROGRAM  ASKS  FOR  UPPER  BOUNDARY{L),   THE  STEP  SIZE(M), 


c 

AND  THE  LOWER  BOUNDARY (K)   OF  THE  FUNCTION 

c 

********** 

c 

WRITE  (6,101) 

101 

FORMAT    ( '    UPPER  LIMIT  OF  SAWTOOTH  =  '/ 
1   2X(0  TO  32767  OUTPUTS  +10  TO  -10  VOLTS) 
READ   (6,100)  L 

100 

FORMAT (15) 
WRITE  (6,102) 

102 

FORMAT   ('   SIZE  OF  STEPS  =  ') 
READ  (6,100)M 
WRITE  (6,103) 

103 

FORMAT   ('   LOWER  LIMIT  OF  SAWTOOTH  =  ') 

C 

c 

READ  (6,100)K 

********** 

C  dERE, 

THE  NESTED  LOOP  LOADS   16  OUTBUF 

C 

REGISTERS   Yilld  THE  APPROPRIATE  VALUES. 

c 

THE  OUTEk  LOOP  CALLS  ARMOUT (16)    TO  OUTPUT 

c 

THESE  VALUES  AFTER  EACH  STEP  INCREMENT. 

c 

********** 

c 

11 

DO  20  I=K,L,M 
DO  10  J=l,16 

10 

OUTBUF(J)=I 
CALL  ARMOUT (16) 

20 

CONTINUE 

c 

21 


(J  ********** 

THIS  GOTO  STATEMENT  CAUSES  THE  STAIRCASE 
C  FUNCTION  TO  REPEAT  UNTIL  THE  PROGRAM  IS 

C  STOPPED  3^  THE  USER 

Q  ********** 

c 

GO  TO  11 
END 
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c 

Q  ************************************* 

c 

C  SNW 

C  This  PROGRAM,   l»vITH  ITS  SUBROUTINE  SAAR,  IS 

C  PART  OF  AN  ACCEPTANCE  PROCEDURE  FOR  THE  MANIPULATOR. 

C  IT  CAUSES  ALL  OF  THE  JOINTS  TO  BE  EXERCISED 

C  SIMULTANEOUSLY   IN  A  RYTHMIC  FASHION. 

C  ALL  OF  THE  OUTPUTS  ARE  SINE  WAVES.  THE 

C  FREQUENCY  OF  EVERY  JOINT  CAN  3£  THE  SAME  OR 

C  DIFFERENT  FROM  EVERY  OTHER  JOINT.      IF  THE 

C  FREQUENCIES  ARE  KEPT  CONSTANT,  THE  PHASE  SHIFT 

C  OF  EACH  JOINT  CAN  BE  DIFFERENT.     THE  AMPLITUDE 

C  OF  THE  MOTION  OF  EACH  JOINT  IS  CONTROLLED  BY 

C  ASSIGNING  LIMITING  VALUES  THROUGH  THE  SUBROUTINE 

C  SAAR.      THE  OVERALL  FREQUENCY  AT  WHICH  THE  SYSTEM  RUNS 

C  IS   DETERMINED  BY  THE  VOLTAGE  LEVEL (A  POTENTIOMETER) 

C  ON  A  DESIGNATED  INPUT  CHANNEL. 

C 

Q  ************************************* 


IMPLICIT  INTEGER  (B-R) 
IMPLICIT  INTEGER  (T-Z) 
COMMON/Z/ZZ 

COMMON/ARMBUF/INBUF (64) ,OUTBUF(64) 

DIMENSION  ASIN(400)  ,K(10)  ,AB(10)  ,B(10)  ,ABB(10)  ,L(10) 

C 


Q  ********** 

C   ZZ   IS   A   INTEGER  THAT,   WHEN   ITS  VALUE   IS   ZERO,  CAUSES 

C  THE  PROGRAM  TO  WAIT  AFTER  THE   INITIAL  VALUES  HAVE 

C  BEEN  SENT  10  ALL  THE  JOINTS.      THE   PROGRAM   PRINTS  OUT 

C  A  STATEMENT  TO  THIS  EFFECT  AND  WAITS   FOR  THE  USER 

C  TO  ENTER  A  +1   IN  ORDER  TO  CONTINUE. 

Q  ********** 


ZZ  =  0 

c 

^  ********** 

C  THIS  SECTION  OF  CODE  SETS  UP  A  TABLE  OF  SINE  VALUES 
C  FROM  1  TO  360  DEGREES  TO  BE  USED  IN  CALCULATING 

C  THE  OUTPUTS. 

Q  ********** 

C 

DO  35  R=l,360 
AR=R 

AX=(6. 28/360. ) *AR 
35  ASIN{R)=SIN(AX) 

C 

********** 

C  REQUESTS  THE   INPUT  CHANNEL  NUMBER (Q)    WHICH  WILL 
C  CONTROL  THE  OVERALL  FREQUENCY  OF  THE  SYSTEM. 

^  ********** 

C 

88  WRITE (6, 90) 

90  F0RMAT(3X 'THE   FREQ  OF  THE  OUTPUT  CHANNELS   WILL  BE  CONTROLLED/ 

1   3X'BY  THE  VOLTAGE  ON  A  REF   INPUT  CHANNEL '/3X ' REF  INPUT 
1  CHANNEL  NUMBER  = ' ) 
READ(6,93)  Q 

93  FORMAT (13) 

C 

2:; 


C  ASKS 

C 

C 

C 

c 
c 

lk)0 


103 

C 
C 

C  BKANC 
C 
C 
C 

c 
c 

C  THIS 

c 
c 
c 
c 
c 

72 
216 
217 
218 


MHtiTHKK  ALL  OP  THE  OUTPUTS  SHOULD  dti  A  CONSTANT 
FKtUUENCJf  WITH  DIPfEKeNT  PHASE  SHI PTS  ( Z--1 )  ,  OR  IF 
ALL  OF  THE  OUTPUTS  SHOULD  BE  A  DIFPERENT  FREQUENCE 
(Z-  +  1)  . 
********** 

WRITt;{6,100) 

FORMAT('        IF  ALL  OUTPUTS  ARE  TO  BE  THE  SAME  FREQ,'/ 

1   3X'BUT  HAVE  DIFFERENT  PHASE  SHIFTS,   TYPE     -1 '/ 

1   BX'IF  EACH  OUTPUT  IS  TO  HAVE  A  DIFFERENT  FREQ,  TYPE 

READ(6,103) Z 

FORMAT (13) 

********** 

HES  TO  7k)  FOR  A  CONSTANT  FREQENCY  WITH  DIFFERING 
PHASE  SHIFTS,   AND  TO  72  FOR  DIFFERENT  FREQUENCIES. 
********** 


+  1) 


219 

C 

c 

C  B(J) 

c 
c 
c 
c 
c 
c 

220 
C 

c 

C  THIS 

C 

C 

C 

c 
c 
c 


IF  (Z)70,70,72 

********** 

IS  THE  BRANCH  FOR  ALL  OUTPUTS  HAVING 
DIFFERENT  FREQUENCIES.     THIS  SECTION  OF  CODE  REQUESTS 
EACH  OUTPUT  CHANNEL  NUMBER(K(J))    WITH  ITS  CORRESPONDING 
FREQUENCY  MULTIPLICATION  FACTOR (AB (J) ) . 
********** 

DO  220  J=l,7 
WRITE(6,216) 

FORMAT ('       OUTPUT  CHANNEL  NUMBER  »') 
KbAD(6,217) K(J) 
FORMAT (13) 
WRITE(6 ,218) K(J) 

FORMAT {30X'   FREQ  FACTOR  FOR  CHANNEL '  , 1 2 ,  '  = '  / 

1   30X'(ANY  FLOATING  POINT  NUMBER  FROM  0.000  TO  99.000)') 

READ(6,219) AB(J) 

FORMAT (F6. 3) 

********** 

IS  THE  INTEGER  VALUE  OF  THE  ANGLE  FOR 
JOINT  J.     ABB (J)    IS  THE  FLOATING  POINT  VALUE 
OF  THE  ANGLE  FOR  JOINT  J.      INITIALIZING  THESE 
TO  A  VALUE  OF  1  FOR  ALL  OF  THE  JOINTS  CAUSES 
ALL  OF  THE  ANGLES  TO  BEGIN  TOGETHER  AT  I  DEGREE. 
********** 

B(J)=1 
ABB(J)=1 
GO  TO  700 

********** 

IS  THE  BRANCH  FOR  ALL  OUTPUTS  HAVING  THE 
SAME  FREQUENCY  BUT  DIFFERENT  PHASE  SHIFTS. 
THIS  SECTION  OF  CODE  REQUESTS  EACH  OUTPUT  CHANNEL 
NUMBER(K(JJ) ) ,   AND  ITS  RELATIVE  PHASE  SHIFT 
VALUE(B(JJ) ) . 
********** 
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7tf  DO  lib  JJ=i , / 

WRITE (b ,111) 

111  FORMAT('       OUTPUT  CHANNEL  NUMBER  »') 
RiiAD(6  ,112)  K  (JJ) 

112  FORMAT (13) 
WRITe(6,113)K(JJ) 

113  FORMAT (30X' PHASE  SHIFT  OF  CHANNEL    '  ,12,''  / 
1   3i3X'(AiMY  ANGLE  FROM  0  TO  +3613)  ') 
Kt,AD(6,114)a(JJ) 

114  FORMAT (14) 
C 

l^i;  ********** 

C   in£   FLOATING   POINT  VALUE ( A3B ( J) )    OF  THE  ANGLE 

C  IS  SET  EQUAL  TO  THE  INTEGER  VALUE (fl(JJ))    OF  THE 

C  ANGLE  WHICH  IS  ALSO  THE  PHASE  SHIFT  ANGLE  OF  JOINT 

C  THE  FREQUENCY  MULTIPLICATION  FACTOR (AS (JJ ) )  OF 

C  EACH  JOINT  JJ   IS  SET  EQUAL  TO  1  SINCE   IN  THIS 

C  PART  OF  THE  PROGRAM,   ALL  OF  THE  OUTPUTS  HAVE 

C  THE  SAME  FkEQUENCY. 

Q  ********** 

C 

AB8(JJ) =B(JJ) 

115  AB(JJ)=1 

C 

^  ********** 

C  THIS  'DO  LOOP',   FROM  STATEMENT  700  TO  400, 

C  CALCULATES  THE  APPROPRIATE  OUTPUT  VALUES 

C  FOR  EACH  OF  THE  SEVEN  JOINTS (JK). 

Q  ********** 

c 

700  DO  400  JK=1,7 

C 

Q  ********** 

C  U=  THt  ANGLE   IN  DEGREES   WHOSE  SINE  WILL  BE 

C  THE  OUTPUT.     CONVERTED  HERE  FROM  A  SUBSCRIPTED 

C  TO  A  NON-SUBSCRIPTED  INTEGER.     THIS   IS  DONE 

C  SINCE  THIS  INTEGER  VALUE  wILL  ITSELF  BE  USED 

C  AS  A  SUBSCRIPT  FORv  THE    'ASIN'  TABLE. 

Q  ********  ** 

c 

D=B(JK) 

C 

^  ********** 

C  CHECKS  TO  SEE   IF  THE  ANGLE (D)    HAS  EXCEEDED 

C  360  DEGREES.     IF  SO,   360   IS  SUBTRACTED  FROM 

C  THE  ANGLE (BOTH  THE   INTEGER  VALUE (D)    AND  THE 

C  FLOATING  POINT  VALUE (ABB (JK) ) .     THIS,  THEN, 

C  BECOMES  THE  NEW  VALUE  OF  THE  ANGLE. 

^  ********** 

C 

IP(360-D) 404,404 ,405 
4<)4  D=D-360 

ABB(JK)=A8B(JK)-360. 

C 

^  ********** 

C  L  BECOMES  THE  OUTPUT  VALUE  FOR  JOINT  JK.  THE 

C  VALUE  OF  THE  SINE  IS  WEIGHTED  TO  PUT  IT  IN 

C  THE   PROPER  OPERATING  RANGE. 

Q  ********** 

c 

405  L(JK) »ASIN(D) *16383. +16383. 
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c 

C  ********** 

C  ABB(JK)    IS  THt  CUKRENT  FLOATING  POINT  VALUE 

C  OF  THE  JOINT (JK)    ANGLE.     A  FLOATING  POINT 

C  VALUE   IS  USED  SINCE  THE  ANGLE  MAY  SOMETIMES 

C  BE   INCREMENTED  BY  A  VALUE  LESS  THAN  ONE. 

C  THIS  OCCURS  WHEN  THE  PRODUCT  OF  AB(JK)*AX 

C  IS   LESS   THAN  ONE.      AB(JK)    MAY  HAVE  BEEN 

C  ASSIGNED  A  VALUE   LESS   THAN  ONE   BY  THE  USER(SINCE 

C  AB(JK)    IS  THE   FREQUENCY   MULTIPLIER  FACTOR) .  THE 

C  VALUE  OF  AX   IS  DETERMINED  BY  THE  VOLTAGE  LEVEL 

C  ON  THE   DESIGNATED  INPUT  CHANNEL (Q)  rtHICH 

C  CONTKOLS  THE  OVERALL  FREQUENCY  OF  THE  SYSTEM. 

C  THUS  AX  MAY   BE   <,=,   OR  >  ONE. 

Q  ********** 

c 

ABB(JK) =ABB(JK) +AB(JK) *AX 
B(JK)=ABB(JK) 

C 

Q  ********** 

C  THE  OUTPUT  CHANNEL  NUMBER(K { JK) )    IS  CHANGED 

C  FROM  A  SUBSCRIPTED  TO  A  N0N-SU3SCRI PTED ( N ) 

C  INTEGER. 

Q  ********** 

N=K(JK) 
4^0  OUTBUF(N)=L(JK) 
CALL  SAAR 

C 

^  ********** 

C  ARMIN  IS  CALLED  TO  BRING  IN  THE  VALUE  OF 

C  THE  REFERENCE   INPUT  CHANNEL (Q)    TO  CONTROL 

C  THE  OVERALL  FREQUENCY  OF  THE  SYSTEM. 

Q  ********** 

C 

CALL  ARMIN 

C 

Q  ********** 

C  THE  VALUE  OF  THE  REFERENCE   INPUT  CHANNEL (Q)  GOES 

C  FROM  \d  TO  +10  VOLTS  PROVIDING   INPUT  VALUES  OF 

C  0  TO  -8h92  RESPECTIVELY. 

C  THE   FOLLOVNiNG  CODE   IS   USED  TO  GENERATE  A 

C  FLOATING   POINT  NUMBER (AX)    THAT  TAKES  ON 

C  VALUES   OF  k).012  TO   11.24.      THIS   NUMBER  IS 

C  MULTIPLIED  BY  THE  DELTA  INCREASE (A8 (JK) )  OF 

C  THE   ANGLE   FOR  EACH  JOINT  TO  PROVIDE  AN  OVERALL 

C  VARIATION   IN  THE  SPEED  THAT  THE   SYSTEM  RUNS. 

C  THAT  IS,   THE  SPEED  OF  THE  SYSTEM   IS  VARIED 

C  BY   HAVING  THE   POINTS   COME   OUT  OF  THE  COMPUTER 

C  AT  A  CONSTANT  RATE,   WHILE  THE  SIZE  OF  THE 

C  INCREASE  OF  EACH  JOINT  IS  CONTROLLED  BY  THE 

C  MAGNITUDE  OF  THE  FACTOR  AX. 

Q  ********** 

c 

IF  (INBUF(Q)+4096)67,67,68 

67  AY=(-1) * (INaUF(Q) +4096) 
AX=(AY/400.)+l. 

GO  TO  3k) 7 

68  AW=INBUF (Q) +4096 

AX=1 ./( ( (Aw/50. ) * (AW/40  96 . ) ) +1 . ) 

307  IF(ZZ) 308,308,700 

308  WRITE(6,301) 

301  FORMAT (10X 'THE   INITIAL  VALUES   ARE   ON  THE  OUTPUT  CHANNELS'/ 


?.r 


i    lOX    IF.YOU    WISH   THt;    PkOGKAM    I'U   CONTINUt;,    TYHt;  +1') 
RbAD  (6  ,  3ld2  )  ZZ 
j02  tJKMAT{I3) 

i  t  (  A  Z )  d  B  ,  7  k)  k)  ,  7  k)  id 
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c 

Q  ************************************* 

c 

C  IiN'i'fC 

C  THIS   IS  AN   INTERFACE  TESTER  PROGRAM'.      ITS  PURPOSE 

C  IS  TO   (1)    MEASURE  THE  AMOUNT  OF  CROSSTALK  BETWEEN 

C  ADJACENT   INPUT  CHANNELS  AND    (2)    MEASURE  THE 

C  ACCURACY  UF  THE   DIGITAL  TO  ANALOG    (D/A) ,   AND  THE 

C  ANALOG  TO  DIGITAL    (A/D)    CONVERSIONS   IN  THE  INTE-UACE. 

C  THE  PROGRAM  REQUESTS  AN  OUTPUT  CHANNEL  NUMBER. 

C  THkOUGH  THIS  CHANNEL,   THE  USER  HAS  THE  OPTION  OF 

C  SENDING   EITHER  A  CONSTANT  VALUE,   OR  A  STAIRCASE 

C  FUNCTION.      THIS   ENTERED  VALUE   PASSES  THROUGH 

C  THE   D/A  ON   THE   SPECIFIED  OUTPUT  CHANNEL,  WHICH 

C  IS  CONNECTED  TO  A  SPECIFIED   INPUT  CHANNEL,  INTO 

C  THE  A/D  AND  BACK  TO  THE  COMPUTER.     THIS  RETURNED 

C  NUMBER   IS   COMPARED  TO  THE   NUMBER  THAT  ImAS  SENT 

C  AND  A  MEASURE   OF  THE   ACCURACY  OF  THE   D/A  AND 

C  A/D  CONVEkSIOnS    IS   OBTAINED.      IN  ADDITION, 

C  THE   VALUES   ON   THE   INPUT  CHANNELS   ON  EITHER 

C  SIDE  OF  THE  SPECIFIED   INPUT  CHANNEL  ARE 

C  READ.      THIS  ALLOWS   A  MEASURE   OF  THE  CROSSTALK 

C  BETWEEN  THESE  CHANNELS. 

C  THIS   PROGRAM  CALLS  THE  SUBROUTINES    :   ARMIN  : 

C  :   ARMOUT  : 

c 

^  ************************************* 

C 

IMPLICIT   INTEGER  (A-Z) 
COMMON/ARMBUF/INBUF (64) ,OUT8UF(64) 

C 

Q  ********** 

C  i'HIS   SECTION  OF  CODE  REQUESTS  THE  OUTPUT 

C  CHANNEL  NUMBER(R),   THE   INPUT  CHANNEL  NUMBER{K), 

C  AND  WHETHER  THE  OUTPUT   IS  TO  BE   A  CONSTANT 

C  VALUE  (U)  (S  =  l)  ,   OR  A  STAIRCASE   FUNCTION  (S  =  k) )  . 

C  . IF  A  STAJRCASE   FUNCTION,    IT  ASKS   FOR  THE 

C  STARTING  VALUE(T),   AND  THE  STEP  SIZE(L). 

Q  ********** 

20  WRITE  (6,100) 

lk)«)  FORMAT    (    '   OUTBUF     CHANNEL  NUMBER=    '  ) 

READ    (6,lidl)  R 
lai  FORMAT  (12) 

WRITE  (6,115) 
115  FORMAT(    '    INBUF  CHANNEL  NUMBER=  ) 

READ  (6,125) K 
125  FORMAT (I  2) 

WRITE  (6  ,15i^)) 

15kJ  FORMAT('    IF  OUTPUT  CHANNEL  IS  TO  HAVE  A  CONSTANT  VALUE,   TYPE  1') 

WRITE(6,151) 

151  FORMAT('    IF   IT  IS  TO  STEP  THROUGH  A  RANGE  OF  VALUES,    TYPE   t)  ' ) 
READ(6,152)  S 

152  FORMAT (14) 
IF(S) 110,110,140 

110  rtRlTE(6,178) 

178  FORMAT('   STARTING   VALUE=  '  ) 

READ(6,179)  T 
17  9  F0KMAT(I7) 

WRITE  (6,109) 
109  FORMAT    (    '        STEP  SIZE=  ') 


KtAD    (6,lkl5)  L 
lv)5  F0kMAT(I7) 

GO  TO  27 
i4tf  AKiit;  (6  , 159) 

159  hOKMA'K'      CONSTANT  VALUe=  '  ) 

Ud AD (6, 167)  U 
lb  7  fOKMAT(I7) 
z  7  J  =  w 

63  I F(S) 7  5,75,76 

7  5  X=T-L*J 

vjO  to  7  7 

76  X  =  U 
C 

********** 

C   ticKc,    J    LS   USED  AS   A  COUNTING   INOcX  THAT 
C  ALLOrtS  THE,   PROGkAN  TO  CYCLfcl   lk)k)  TIMES. 

C  THt;   PkOGRAM  I'titu   STARTS   OVER  AT  THE 

C  BEGINNING. 
^  ********** 

C 

77  J=J+1 

IF  (lt)0-J)  64,63  ,63 

C 

^  ********** 

C  Trie  SPECIFIED  OUTBUF  REGISTER   IS  LOADED 

C  WITH  THE   PROPER  VALUE 

^  ********** 

C 

63  0UT3uF(R)=X 

CALL  ARM0UT(16) 

VvRITE    (b,lB2)  R,OUTBUF(R) 
1^2  FOkMAT(22X    '0UT6UF(  '  ,12,  ')  =  '  ,17) 

CALL  AKMIN 

C=K-1 

U  =  K  +  i 

C 

(J  ********** 

C   TriL  VALUt,   ON   THE  SPECIFIED   INPUT   CHANNEL  AS 

C  WELL  AS  THE  VALUES  ON  THE  CHANNELS   ON  EITHER 

C  SIDE   ARE  READ   IN   AS    IN3UF(K)  ,    INBUF(C)  ,  AND 

C  INciuF(D).      THESE   VALUES  ARE  THEN 

C  WEIGHTED  TO  liflELD  THE   INTEGERS   N  ,  i"l ,  P  ,  WHICH 

C  CAN   BE  COMPARcD  TO  THE  NUMBER  SENT  ON  THE 

C  OUTPUT  CHANNEL. 

^  ********** 

C 

t\=2*INBUF  (K)  +  16384 
M=2*INBUF(C)  +  16334 
P=2*1NBUF(D)    +  16384 

C 

Q  ********** 

C  THIS  SECTION  WRITES  OUT  THE  INPUT  CHANNEL  NUMBERS (C , K , D) , 
C  AND  THEIR  WEIGHTED  VALUES ( M , N , P) . 

Q  ********** 

c 

WRITE(6,lk33)  C,M,K,N,D,P 
lk)3  F0RMAT(3(    '       INBUF{  '  ,12,  ')  =  '  ,17)  ) 

C 
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c 

C  THE 

C 

C 

c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 
c 


13k) 

64 
2^k) 


2kJl 


********** 

DIFFERENCE  BETWEEN  THE  OUTPUT  VALUE (X) 

SENT  TO  THE   D/A  AND  THE  VALUE (N)  RETURNED 
FROM  THE  A/D  IS   COMPUTED  AND  PRINTED  OUT 
AS  THE   bKROR{ERR) .      THIS   IS   A  MEASURE  OF 
THE  ACCURACY  OF  THE  CONVERSION  PROCESS. 
A  MEASURE  OF  THE  NOISE   LEVEL  OF  THE  SYSTEM 
IS  OBTAINED  BY  OBSERVING  THE  VARIATION  IN 
THE   ERROR  SIGNAL  OVER  A  NUMBER  OF  TRIALS. 

THE   MAGNITUDE  OF  THIS   ERROR  CAN   BE  COMPARED 
TO  THE   cKROR  GENERATED  WHEN  THE  MANIPULATOR 
IS    IN   THE    LOOP,    THEREBY,    PROVIDING  A  MEASURE 
OF  THE  ACCURACY  OF  THE  HARDWARE  SERVOS  OF 
THE  ARM. 
********** 


£RR=   N   -  X 

WRITE (6 ,130)  ERR 

FORMAT (55X    ' ERR0R= '  , I  7 ,///) 

GO  TO  68 

WRITE(6,200) 

FORMAT (3X  TO  CONTINUE,    TYPE  (CR).'/ 

1    3X'T0  SELECT  ANOTHER   INPUT  CHANNEL,'/ 

1   3X  'TYPE  +1 .  '  ) 

kEAD(6,2t)l)  J 

FORMAT (I  3) 

IF(J) 77 ,77 ,20 

END 
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SU6K0UTINE  SAAR 

C 

^  ************************************* 

C 

C  THIS   SUdROUTINt;   IS   USED  WITH  THE   PROGRAM  SNW 


C  TO  GENERATE  SINE  WAVE  OUTPUTS  TO  THE  JOINTS 

C  OF  THE  MANIPULATOR.      THE   PURPOSE  OF  THIS 

C  SUBROUTINE   IS      (1)    TO  PREVENT  THE  ARM  FROM 

C  TRAVELING  TOO  GREAT  A  DISTANCE   IN  A  SINGLE 

C  STEP  BY   PRESCRIBING  THE   LARGEST  AMOUNT 

C  THAT  THE  OUTPUT  VALUE   IS   ALLOWED  TO  CHANGE, 

C  AND    (2)    TO  SET  LIMITS   ON   THE   AMPLITUDE  OF  THE 

C  SINE  WAVE  TO  EACH  JOINT. 

C 

Q  ************************************* 


C 

IMPLICIT  INTEGER  (A-V) 
IMPLICIT  INTEGER  (X-Z) 
COMMON/Z/ZZ 

COMMON/AKMBUF/INBUF (64) ,OUTBUF(64) 
DIMENSION  AOTBF(64),  CHAN(2a,2) 

C 


Q  ********** 

C  ZZ   IS  Trio   FLAG   FROM  THE  SNW   PROGRAM  THAT 

C  HAS  A  VALUE  OF   \d   FOR  THE   INITIAL  OUTPUT, 

C  AND  +1   FOR  THE  REMAINDER  OF  THE  PROGRAM  RUN. 

C  ZZ   IS  USED  HERE  TO  PREVENT  AN  ERROR 

C  SIGNAL  FROM  OCCURRING  WITH  THE  INITIAL 

C  OUTPUT.     THE  ERROR  SIGNAL  ARISES  WHENEVER 

C  THE  DIFFERENCE  BETWEEN  THE  PRESENT  OUTPUT  AND 

C  THE  PREVIOUS  OUTPUT  IS  GREATER  THAN  THE 

C  SPECIFIED  ALLOWABLE  CHANGE(B).  FOR 

C  THE  INITIAL  OUTPUT,   THE  PREVIOUS  OUTPUT 

C  IS   ZERO,   THEREFORE,   THE  DIFFERENCE  WILL 

C  PROBABLY  BE  GREATER  THAN  THE  SPECIFIED  VALUE (B). 

C  THUS,   FOR  ZZ=0,   THE  PREVIOUS  OUTPUT  IS 

C  SET  EQUAL  TO  THE  INITIAL  OUTPUT  BEFORE 

C  THE   DIFFERENCE  BETWEEN  THEM   IS  CALCULATED. 

Q  ********** 

C 

IF(ZZ)4,4,5 

C 

Q  ********** 

C  THIS   BLOCK  OF  CODE    (STATEMENT  4  THROUGH   10)  IS 

C  EXECUTED  FOR  THE  INITIAL  OUTPUTS  ONLY.  THIS 

C  SECTION  OF  CODE  REQUESTS    (1)    THE  SIZE  OF  THE 

C  ALLOWABLE  OUTPUT  CHANGE(B),    (2)    IF  LIMITS 

C  ARE  TO  BE  SET  ON  THE  AMPLITUDES  OF  THE  SIGNALS 

C  TO  ANY  OF  THE  JOINTS    { YES ( LIMIT=-1 ) ,   NO ( LIMIT=+1 ) ) 

C  AND    (3)    IF  LIMITS  ARE  TO  BE  SET,    IT  ASKS  FOR 

C  THE  LOWER  LIMIT ( CHAN ( I J , 1 ) )    AND  THE  UPPER 

C  LIMIT(CHAN(IJ,2) )    FOR  EACH  JOINT(IJ). 

C  THIS  SECTION  OF  CODE  ALSO  SETS  THE  PREVIOUS 

C  OUTPUT  VALUES (AOTBF (J) )    EQUAL  TO  THE  PRESENT 

C  INITIAL  VALUES (OUTBUF (J) )    FOR  EACH  JOINT(J) 

C  FOR  THE  REASON  DESCRIBED  ABOVE. 

Q  ********** 

C 

4  WRITE(6,227) 

227  F0RMAT(3X,  'SIZE  OF  LARGEST  ALLOWED  OUTPUT  CHANGE=',/, 
1   3X,'(0  TO  32767  GIVES  +10  TO  -10  VOLTS)') 
READ(6,228)  B 

228  FORMAT (I  5) 
C 

3] 


Q  ********** 

C  THIS  DO  LCX3P'  SETS  THE  LOWER  (CHAN  ( KL  ,  1 )  AND 

C  THE  UPPER(CHANN(KL,2)    LIMITS  OF  EACH  CHANNEL(KL) 

C  TO  THEIR  NORMAL   FULL  RANGE  VALUES 

C  OF  k)  AND  32  ,  767  RESPECTIVELY. 

Q  ********** 

c 

DO  13   KL  =  1,20 
CHAN (KL,1) =0 
13  CHAN (KL, 2) =32767 

WRITE(6,111) 

111  FORMAT ( 3 X,   ARE  ANY  CHANNELS  TO  BE   LIMITED  ?',/, 
1   3X,'IF    'YES"   TYPE  -1,     IF   "NO"  TYPE  +1') 
R£AD(6,112)  LIMIT 

112  FORMAT (I  5) 
IF(LIMIT.N£.-1)    GO  TO  5 
WRITE(6,999) 

999  FORMAT (3X, 'SET  LIMITS  BETWEEN  0  AND  32767',/, 

1   3X,'GIVE  THE  LOWER  LIMIT  FIRST') 
DO  9   IJ  =  1,7 
WRITE(6,113)  IJ 

113  F0RMAT{3X,  'LIMITS  ON  CHANNEL (', 1 1 ,' )  ARE') 
KEAD(6,114)    CHAN(IJ,1),  CHAN{IJ,2) 

114  FORMAT (I  5) 
C 

Q  ********** 

C  THIS  SECTION  OF  CODE  MODIFIES  THE  SINE  WAVE 
C  OUTPUT  SO  THAT  THE   FULL  SWEEP  OCCURS 

C  WITHIN  THE  LIMITS  SET  FOR  EACH  JOINT. 

C  W  IS  SET  EQUAL  TO  THE  SINE  WAVE  OUTPUT 

C  VALUE (OUTBUF(IJ)    FOR  JOINT  I J .     WI   IS  SET 

C  EQUAL  TO  THE  LOWER  LI MI T ( CHAN ( I J , 1 )  FOR 

C  JOINT  IJ.   WIL  IS  SET  EQUAL  TO  THE  ALLOWED 

C  AMPLITUDE  RANGE   BETWEEN  THE  LOWER  AND 

C  UPPER  LIMIT  FOR  JOINT  IJ .  THE 

C  NEW  OUTPUT  IS  THEN  CALCULATED  SO  THAT  THE 

C  FULL  AMPLITUDE  OF  THE  SINE  WAVE  OCCURS 

C  BETWEEN  THESE  SPECIFIED  LIMITS. 

Q  ********** 

c 

W=OUTBUF(IJ) 

WI=CHAN(IJ,1) 

WIL=CHAN (IJ ,2) -CHAN (IJ  ,1) 

OUTBUF(IJ) =W*WIL/32767 .+WI 

9  CONTINUE 

DO  10  J  =  1,16 

10  A0T6F(J) =OUTBUF(J) 
C 

********** 

C  THIS   BLOCK  OF  CODE    (FROM  STATEMENT   5  TO  THE  END) 

C  IS  THE  PART  OF  THE  SUBROUTINE  CALLED  AFTER 

C  THE   INITIAL  VALUES   HAVE   BEEN  SENT.  IT 

C  CORRECTS  THE  AMPLITUDE  OF  THE  SIGNAL  TO 

C  EACH  JOINT  IN  ACCORD  WITH  THE  LIMITS, 

C  AND  LOOKS   FOR  AN  ERROR  IN  THE  SIZE  OF  THE 

C  OUTPUT  CHANGE.      IF  AN  ERROR  DOES  OCCUR, 

C  THE  PROGRAM  STOPS,    PRINTS  THE  SIZE 

C  OF  THE   ERROR  AND  THE  CHANNEL  NUMBER  IT 

C  IS  ON  AND  WAITS  FOR  A   [CR]    BEFORE  CONTINUING. 

Q  ********** 
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c 

5  DO  50  J=l,16 

W=OUTBUF  (J) 

VvI=CriAN  (J  ,1) 

wIL=CriAN (J , 2) -CHAN ( J , 1) 

OUTBUF (J) =rt*WIL/327  6  7 .+WI 

X=AOTdF ( J) -OUTBUF ( J) 

IF  (X)25,5ia,17 
25  X=-X 
17  IF  {a-X)l,2,2 

1  WRITE(6,200) J,X 

200  FORMAT{'     DELTA(  '  ,  12  ,  ') «  ',16) 
WRITe(6,201) 

201  FORMAT ('     TO  CONTINUE,   PUSH  THE  CARRIAGE  RETURN') 
R£AD(6,202)M 

202  FORMAT (16) 

2  AOTBF (J) =OUTBUF  ( J) 
50  CONTINUE 

CALL  ARM0UT(16) 
END 
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Appendix  E:    ROBOT  CONTROL  PROGRAMS 


These  programs  allow  robot  control  of  a  manipulator  by  a  mini-computer.  Input 
commands  are  read  from  a  master  (referred  to  as  the  exoskeleton  in  the  documentation)  and 
output  commands  drive  the  servos  of  the  slave  manipulators. 

1.  SANDF:       "Store  and  forward"  for  master/slave  control.    Reads  input  commands, 

scales  them,  and  outputs  them  to  the  slave  manipulator. 

2.  ARM:         Basic  robot  program  with  continuous  path  teaching  and  playback. 

3.  PNTPNT:     Point-to-point  version  of  ARM. 

4.  NEWARM:     Combines  ARM  and  PNTPNT  and  adds  interpolation  If  desired  on  playback, 


Note: 


SANDF  needs  to  know  which  input  channels  are  which  and  what  scaling 
factors,  if  any,  are  needed  to  produce  the  proper  output  commands 
to  the  servos  (e.g.  in  the  NBS  system  the  D/A  converters 
require  all  positive  numbers;  the  value  +16,384  is  zero  volts). 


Note:  NEWARM  is  the  recommended  robot  program  for  general  use. 
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SUBROUTINE  SANDF 

C 

C  SANDF  STANDS   FOR  STORE  AND  FORWARD 

C  IT  STORES,   SCALES  AND  FORWARDS  THE  VALUES  READ 

C  FROM  THE  EXOSKELETON   (A/D  CONVERTED  VALUES)  TO 

C  THE  SLAVE  ARM    (D/A  CONVERTED  VALUES) . 

C  ALL   VALUES   MUST  dE  SCALED  TO  POSITIVE  QUANTITIES 

C  AS  THE  SIGN  BIT  OF  A  NUMBER  SENT  TO  THE  D/A  CONVERTER 

C  WILL   RESET  THE  CHANNEL  ADDRESS  TO  id. 

C 


C  THIS   PROGRAM  IS   USED  TO  GATHER  ALL  64   CHANNELS   INTO  THE  COMMON 

C  AREA  ARMBUF   IN  ARRAY   INBUF,    THEN  OUTPUT  THE  SCALED  VALUES 

C  FROM  THE  MASTER  EXOSKELETON.   THERE  ARE   7   DEGREES  OF  FREEDOM. 

C  THE   64  CHANNELS   INCLUDE   MASTER  POSITION   POTS,   SLAVE  POSITION 

C   POTS  SENSORS,    AND  SENSE  SWITCHS. 

C 

IMPLICIT  INTEG£R{A-Z) 

COMMON  /ARMBUF/INBUF(64) ,OUTBUF{64) 

C 

CALL  ARMIN 

C 

Q  ********** 

C  THE  FOLLOWING  TWO    'DO  LOOPS'   ARE  USED  TO 

C  GENERATE  THE  CORRECT  INPUT  CHANNEL  NUMBERS 

C  (5,8,11,13,15,17,19)    FOR  THE  SEVEN  JOINT 

C  POTENTIOMETERS. 

Q  ********** 

c 

K  =  2 

DO  50  1=1,2 
K  =  K  +  3 

OUTBUF(I) =IABS (INBUF(K) *  2  +  16 33 3) 

50  CONTINUE 
K  =  9 

DO  51  1=3,7 
K  =  K+2 

UUTBUF(I)=IABS(INBUF(K) *  2  +  1 6 38 3) 

51  CONTINUE 
C 

RETURN 
END 


35 


C  ARM 

C 

c 

C       AKM   IS   A   PKOGKA.^  TO  STUKL  ANU  FORWARD   DATA  BE;TWt;t;N 

C  AN   INPUT  CONTROL  DEVICE    ( EXO-SKELETON)    AND  A  MECHANICAL 

C  MANIPULATOR.      AT  THE  OPERATOR'S   REQUEST,   ARM  RECORDS 

C  THE   INPUT  DATA  FOR  SUBSEQUENT  PLAYBACK  —  MUCH  LIKE 

C  A  TAPE  RECORDER. 

C 

IMPLICIT  INTEGER  (A-Z) 

C 

C       DATA   IS   PASSED  BETWEEN  THE  MAIN   PROGRAM  AND  THE  LOwER 
C  LEVEL   INTERFACE   DRIVER  ROUTINES  THROUGH  THE  COMMON 

C  DATA  AREA  NAMED  ARMBUF. 

C 

COMMON/ARMBUF/INBUF (64) ,OUTBUF(64) 

C 

C  THE  LOCAL  BUFFER  INC8UF  STORES  THE  INPUT  COORDINATES  DURING 

C  THE  RECORDING  SESSION.      AT  THE  OPERATOR'S  REQUEST,  THIS 

C  RECORDED  DATA  CAN   BE  SAVED  ON   DISK  OR  TAPE. 

C 

DIMENSION   INCBUF(200t)  ,7) 

C 

C        FOR  CONVENIENCE,   A  VARIABLE  NAMED  SWITCH   IS  EQUIVALENT  TO  THE 

C  INPUT   BUFFER  ELEMENT  ASSIGNED  TO  CHANNEL  NUMBER  27. 

C  THIS   CHANNEL   ENABLES  THE  OPERATOR  TO  COMMUNICATE  TO 

C  THE  RUNNING   PROGRAM  VIA  A  FUNCTION   BUTTON  LOCATED  NEAR  THE 

C  INPUT  EXO-SKELETON. 

C 

EQUIVALEnC£(SWITCH,INBUF(27) )  '  ' 

C 

C       FIRST,   THE   PROGRAM  ACCEPTS  A  REQUEST  FROM  THE  OPERATOR 
C  TO  PLAY  BACK  A  PREVIOUSLY  RECORDED  TRAJECTORY. 

C 

WRITE (6 ,1000)  .  ' 

READ(6,1100) FILFLG 

IF    (FILFLG     .N£.    1HY)G0T0  10 

C 

C        IF  THE  OPERATOR  DID  REQUEST  A   PREVIOUSLY  RECORDED  TRAJECTORY 
C  THE  OLD  DATA  VALUES  ARE   READ  IN  AND  CONTROL  TRANSFERS 

C  TO  THE   PLAYBACK   PORTION  OF  THE  PROGRAM. 

C 

WRITE(6,1010) 
DO  2  1=1,2000 

2  R£AD(2,1080,END=3) (INCBUF(I,J) ,J=1,7) 

3  IINC=I-1 
GOTO  3  00 

C 

C        IF  THE  OPERATOR  INTENDS  TO  CREATE  A  NEW  TRAJECTORY,   THE  PROGRAM 

C  FIRST  ENTERS   A  LOOP   IN   WHICH  ALL  OF  THE  JOINT  ANGLES 

C  FROM  THE   EXO-SKELETON  ARE   FORWARDED  TO  THE  MANIPULATOR. 

C  ALSO,    THE   SWITCH    (CHANNEL   27)    USED   BY  THE   OPERATOR  IS 

C  SAMPLED  OlN   each   PASS   THROUGH  THE  LOOP. 

C  A  COMBINATION  OF  THE  SWITCH  VALUE   AND  THE  VARIABLE  LATCH 

C  IS   USED  TO  CONTROL  THE   PROGRAM  FLOW. 

C 

10  LATCH=1 
IINC=1 
EOTHR=2  0 
EOTHRS=EOTHR 
WRITE(6,1020) 
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c 

C  TriE  SUBROUTINE  SANDF   IS   CALLED  TO  STORE   AND  FORWARD  THt  JOINT 

C  ANGLES   FROM  THE   INPUT  EXO-SKELETON  TO  THE  MANIPULATOR.  THIS 

C  CALL   IS   REPEATED   Ut^iTIL   SWITCH   CHANGES   STAi'L  . 

C 

11  CALL  SANDF 

C 

C  A  COMdINATION  OF  SWITCH  AND  LATCH   IS   USED  TO  DECIDE 
C  TriE   NEXT  COURSE  OF  ACTION. 

IF(SrtITCri    .GT.    lk)00)    GOTO    ( 1 1  la  ,  1 2  0  , 1  3  0  , 1  4  0  , 1 5 '.i )  ,  LATCH 
GOTO   (  21kJ  ,  220  ,  230  ,  240  ,  250)  , LATCH 

C 

C  aHEN  RECOkDIN'G  begins,  THE  OPERATOR  IS  INFORMED  THAT  EOTHR 
C  (EVERY  OTHER)    POINT  wTLL   BE  STORED  AS   A  RECORDED  POINT 

C  IN  THE  TRAJECTORY. 

C 

110  rtRITE (6 , 1030) EOTHR 

LATCri=2 

120  E0THRS=E0THRS-1 
IF(EOTHRS)  121,121,11 

121  DO  122  JINC=1,7 

122  IdCBUF (IINC , JINC) =OUTBUF (JINC) 
EOTHRS=EOTriR 

IINC=IINC+1 

IFdINC    .LE.    2000)    GOTO  11 

C 

C       IF   THE   LOCAL   BUFFER  USED  TO  STORE  THE   RECORDED  TRAJECTORY 

C  SHOULD  OVERFLOW,    THE  OPERATOR  IS  INFORMED. 

C 

LATCH  =3 

WRITE (6 ,1040) 

IINC=IINC-1 
130  GOTO  11 

14m  latch  =  5 

C 

C       AricN  TriE  TRAJECTORY   IS   FINISHED  THE  OPERATOR  IS   INFORMED  AND 

C  ASKED   IF  riE  WOULD  LIKE  TO  SAVE  THE  RECORDED  POINTS. 

C 

WRITE {6 ,1050) 

READ(6,1100) FILFLG 
150  GOTO  11 

C 

210  GOTO  11 

223  IINC=IINC-1 

rtRITE{6,1060) 
230  LATCH  =4 

24^  GOTO  11 

250  IF(FILFLG   .NE.    1HY)G0T0  300 
WRITE (6,107  0) 

DO  251  I=1,IINC 

251  wRITE(2,1080)  (INCBUFd  ,J)  ,J  =  1  ,7) 
C 

300  aRITE(6  ,  1090)  (INC6UF(1,J)  ,J  =  1,7)  ,IINC 

R£AD(6, 1100) FILFLG 
310  DO  320  I=1,IINC 

C 

DO  321  J=l,7 
321  OUTBUF (J) =INCBUF (I , J) 

CALL  ARrtOUT(7) 

C 
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CALL  ARMIN 


C 

C        INPUT   CHANNEL   26   IS   CONNECTED  TO  A   POTENTIOMETER  WHICH  IS 

C  USED  TO  CONTROL  THE  SPEED  AT  WHICH   RECORDED  TRAJECTORIES 

C  ARE   PLAYED   BACK.      TIMOUT   DELAYS   EXECUTION   OF   THE  PROGRAM 

C  FOR  THE  SPECIFIED  TIME. 

C 

CALL  TIMOUT( (IABS( (INaUF(26)/2000)+4)+l) ) 
320  CONTINUE 
GOTO  31M 

C 

1000  FORMAT    ( '   ARM  TRAJECTORY   RECORD/PLAYBACK  DEMON 

ISTRATION   PROGRAM.'/'    DO  YOU   WANT  AN   OLD  TRAJECTORY?') 
1010  FORMAT('    READING   DATA  VALUES,    PLEASE  WAIT') 

1020         FORMAT('   TOGGLE  SWITCH  TO  RECORD  TRAJECTORY') 
1030         FORMAT{'   BEGIN   RECORDING  EVERY    ',12,'  POINTS) 

1040         FORMAT('   FILE  OVERFLOW,'/'   TRUNCATION  OF  RECORDED  TRAJECTORY 

1   OCCURED:    BE  EXTREMELY  CAREFUL  ON  PLAYBACK!') 
1050         FORMAT('   DO  YOU  WANT  TO  SAVE  THIS  TRAJECTORY?') 
1060         FORMAT('   END  RECORDING  TRAJECTORY.   TOGGLE  SWITCH  TO 
IPLAY  BACK') 

1070  FORMAT('    PLEASE   WAIT  WHILE   FILE   IS   BEING  SAVED') 

1080         F0RMAT(7 (I6,2X) ) 

1090         FORMAT!'   THE   INITIAL  ARM  POSITION   IS  '  , / , { 7 ( I  6 , 2X) )  , / 

1,  '   THERE  ARE ',15,'    POSITIONS    In  TrilS  TRAJECTORY',/ 

2,  '   TYPE   CARRIAGE   RETURN    [CR)    WHEN  READY.') 

C 

1100         FORMAT (Al) 
END 
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C  PNTPNT 
C 

C        PiMTPNT  lb   A   PR'JGKA.-l  TO  GENERATE  TRAJECTORIES   BY  ALLOWING  THE 

C  OPERATOR  TO   POSITION   THE   MANIPULATOR  WITH  THE   INPUT  CONTROL 

C  DEVICE  —  THE  £XO-SKELETOW .      ONCE   POSITIONED   3Y  THE 

C  OPERATOR,    THE   MANIPULATOR  COORDINATES   ARE   RECORDED  \vdEN  ' 

C  THE   OPERATOR   PRESSES   A   FUNCTION   BUTTON   LOCATED   NEAR  THE 

C  EXO-SKELETON . 

C 

IMPLICIT   INTEGER  (A-Z) 

C 

C        DATA  IS   PASSED   BETWEEN  THE  MAIN   PROGRAM  AND  THE  LOWER 
C  LEVEL   INTERFACE  DRIVER  ROUTINES   THROUGH  THE  COMMON 

C  DATA  AREA  NAMED  ARMBUF. 

C 

COMMON/ ARMBUF/IN3UF (64) ,OUTbUF (64) 

C 

C       THE   LOCAL   BUFFER   INCBUF  STORES  THE   INPUT  COORDINATES  DURING 
C  THE  RECORDING  SESSION.      AT  THE  OPERATOR'S   REQUEST,   THIS  ' 

C  RECORDED   DATA  CAN  BE  SAVED  ON  DISK  OR  TAPE. 

C 

DIMENSION   INC6UF(2i)0fcJ  ,7) 

C 

C       FOR  CONVENIENCE,    A  VARIABLE   NAMED  SwITCH   IS   EQUIVALENT  TO  THE 

C  INPUT  BUFFER  ELEMENT  ASSIGNED  TO  CHANNEL  NUMBER  27. 

C  THIS   CHANNEL   ENABLES   THE  OPERATOR  TO  COMMUNICATE  TO 

C  THE  RUimNING  PROGRAM  VIA  A  FUNCTION   BUTTON   LOCATED  NEAR  THE 

C  INPUT  EXO-SKELETON. 

C  FUNCTION   dUTTO;nJ   2   IS  ASSIGNED  TO  INPUT  CHANNEL  NUMBER  28. 

C  THIS   FUiMCTION   dUTTON   EQUIVALENT  TO  POINT,    IS   USED  TO 

C  IDENTIFY  A   PARTICUAR  SET  OF  COORDINATES  TO  BE  RECORDED. 

C 

£QUIVALENCE(SWITCH,INBUF(27) ) 
EQUIVALENCE(POINT,INBUF(28) ) 

C 

C        FIRST,    THE  PROGRAM  ACCEPTS  A  REQUEST  FROM  THE  OPERATOR 

C  TO  PLAY   BACK  A  PREVIOUSLY   RECORDED  TRAJECTORY. 

C 

wRIT£(6,lk)0fcl) 

READ(6,1H)0)  FILFLG 

IF    (FILFLG    .NE.    1HY)G0T0  10 

C 

C       IF  THE  OPERATOR  DID  REQUEST  A  PREVIOUSLY  RECORDED  TRAJECTORY 
C  THE  OLD  DATA  VALUES   ARE  READY   IN  AND  CONTROL  TRANSFERS 

C  TO  THE   PLAYBACK   PORTION  OF  THE  PROGRAM. 

C 

wRITE(6,101W) 

DO  2   1=1,2000  i 

2  READ(2,1030,END=3)  (INCBUFd  ,J)  ,J  =  1,7) 

3  IINC=I-1 

GOTO  3  00 

C 

C        IF  THE  OPERATOR  INTENDS  TO  CREATE  A  NEW  TRAJECTORY,   THE  PROGRAM 

C  FIRST  ENTERS  A  LOOP  IN  wHICH  ALL  OF  THE  JOINT  ANGLES 

C  FROM  THE   EXO-SKELETON  ARE   FORWARDED  TO  THE  MANIPULATOR. 

C  ALSO,   THE  SWITCH    (CHSNNEL   27)    USED  BY  THE  OPERATOR  IS 

C  SAMPLED  ON   EACH   PASS   THROUGH  THE  LOOP. 

C  A  COMBINATION  OF  THE  SWITCH  VALUE  AND  THE  VARIABLE  LATCH 

C  IS   USED  TO  CONTROL  THE   PROGRAM  FLOW. 
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la  LATCH=1 
IINC=1 

WRITE  {6  ,  l«:<i0) 

c 

C       Tiln-  SUtiKUU'i'INb  SANDF   IS   CALLLIU  TO  STOKE  AKC  r  OU-VAI^U  THE  .]JIWT 

C  ANGLES   FkOH  THE   INfUT  EXO-SKELLTON    I'D  THE  MANIPULATOR.  THIS 

C  CALL   IS   REPEATED  UNTIL  SWITCH   CHANCES  STATE. 

C 

11  CALL  SANDF 

C 

C       A  C0M3INATI0N  OF  SWITCH  AND  LATCii   IS   USED  TO  DECIDE 

C  THE  NEXT  COURSE  OF  ACTION. 

C 

IF(SaITCH   .GT.    Ik}a0)    GOTO   ( lie)  ,  12k)  ,  13il  ,  14k)  ,  150)  , LATCH 
GOTO   (21k), 22»),23ll,24H, 250)  , LATCH 

C 

C       WHEN   RECORDING   BEGINS,   THE  OPERATOR   IS   INFORMED  THAT  FUNCTION 
C  dUTTON   WUMdER  2,    (INPUT  ON  CHANNEL  NUM3ER   2d)    IS   USED  TO 

C  RECORD  THE   LOCATION  OF  THE  MANIPULATOR  AT   ITS  CURRENT 

C  POSITION. 
C 

Ilk)  rtRITE(b,lk)30)  • 

LATHC=2 

12i3  IF(POINT    .LT.    1U00)GOTO  11 

121  DO  122  JINC=1,7 

122  IWCauF (IINC, JINC) =OUTBUF(JINC) 
E0THkS=E0THK 

IINC=IINC+1 

IF(IINC    .LE.    2000)    GOTO  11 

C 

C       IF  THE   LOCAL   BUFFER  USED  TO  STORE  THE   RECORDED  TRAJECTORY 

C  SHOULD  OVERFLOW,   THE  OPERATOR   IS  INFORMED, 

C 

LATCH   =  3 

WRITE  (6  , 1040)  ■    '  ' 

IINC=IINC-1  '  ^ 

13a  GOTO  11 

140  LAICH   =   5  .      .  ■  .  . 

C       WHEN  THE  TkAJECTOkY   li)   FINIbHED  THE  OPERATOR   IS    INFORMED  AND 

C  ASKED   IF   HE   wOULD  LIKE  TO  SAVE  THE   RECORDED  POINTS. 

C 

WRITE (6 ,1050) 

Rt,AD(6,ll00)  FILFLG 
150  GOTO  11 

C 

210.  GOTO  11  .1 

220  IINC=IINC-1 

WRITE ( 6  , 1060) 
230  LATCH   =  4 

240  GOTO  11 

250  IF (FILFLG   .N£.    1HY)G0T0  300 
WRITE (6, 107  0) 

DO  251   1=1, IINC 

251  WRITE(2  ,1080)  {INCBUF{I,J)  ,J  =  1,7) 
C 

30k)  vJRITE  (6  ,1090)  (INCBUF(1,J)  ,J  =  1,7)  ,IINC 

READ (6, 11 00) FILFLG 
310  DO  320   1  =  1, IINC 

C 
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DU   iZi.   J  =  i  ,  / 
321  OUTBUt'(J)  =INC6UF(I  ,J) 

CALL  AKM0UT(7) 

C 

CALL  ARMIN 

C 

C       INPUT  CHANNlL   26   IS  CONNECTeO  TO  A  POTENTIOMETt: R  /iHlCH  IS 

C  USfciD  TO  CONTROL  THE  SPEED  AT  kvHICH   RECORDED  TRAJECTORIES 

C  ARE  PLAYED  BACK.      TIMOUT  DELAYS   EXECUTION  OF  THE  PROGRAM 

C  FOR  THE  SPECIFIED  TIME. 

C 

CALL  TIM0UT(  (lAbS  (  ( INdUF  (26) /2K)L)t3) +4) +1)  ) 
32\d  CONTINUE 
GOTO  31kJ 

C 

lWK)i3  FORMAT('    ARM  TRAJECTORY   RECORD/PLAYBACK  DEMON 

ISTRATION    PROGRAM.'/'    DO  YOU   wANT  AN  OLD  TRAJECTORY?') 
lt)ltf  FORMAT  ('    READING   DATA  VALUES,    PLEASE  i^iAIT') 

ik)2J  FORMAT('    TOGGLE  SWITCH  TO  RECORD  TRAJECTORY  '  ) 

1U3U  FORMAT('    POSITION  THE  MANIPULATOR  TO  THE  DESIRED  COORDINATES', 

1'   THEiM,    PUSH  BUTTON   2  TO  RECORD  THE   POINT.  ) 

lt)4w  FORMAT{'    FILE  OVERFLOW,'/'    TRUNCATION  OF   RECORDED  TRAJECTORY 

1   OCCUREDibE   EXTREMELY  CAREFUL  ON  PLAYBACK!') 

lk)5fc)  FORMAT('    DO  YOU   WANT  TO  SAVE  THIS   TRAJECTORY?  '  ) 

lU6kj  FORMAT('    END  RECORDING  TRAJECTORY.      TOGGLE  S.vITCH  TO 

IPLAY  BACK) 

ik57K)  FOhMAT('    please  v^AIT  aHILE   FILE   IS   BEING  SAVED '  ) 

llddti  FORMAT  (7  (16,  2X)  ) 

ia9td  FORMAT  ('   THE   INITIAL  ARM  POSITION   I S  '  , /  ,  (  7  ( 1 6  ,  2X)  )  ,  / 

1,  '   THERE  ARE ',15,'    POSITIONS   IN  THIS  TRAJECTORY',/ 

2,  '   TYPE  CARRIAGE   RETURN    [CR]    WHEN  READY.') 

C 

llWkJ  FORMAT  (Al) 

END 


41 


c 

Q  ************************************* 

c 

C  NEWARM 

C  THIS   PROGRAM   IS   A  MODIFIED  VERSION  OF  THE  ARM 

C  PROGRAM.      A  LARGE   PART  OF  THE  CODE,  THEREFORE, 

C  IS   IDENTICAL  AND  WILL  NOT   BE   DOCUMENTED  HERE. 

C  LIKE  ARM,   THIS   PROGRAM  ALLOWS 

C  OPERATOR  TO  RECORD  A  DESIRED  TRAJECTORY  AND 

C  EITHER  PLAY   IT  BACK   IMMEDIATELY  OR  STORE  IT 

C  ON   DISC  TO  dE   PLAYED  BACK  AT  A  LATER  TIME. 

C  RECORDING  THE  TRAJECTORY  CONSISTS   IN  STORING 

C  '  THE  VALUES   OF  THE  POTENTIOMETERS  ON   EACH  OF  THE 

C  .  JOINTS  AS  THE  MANIPULATOR  MOVES   THROUGH  A  SEQUENCE 

C  OF  MOVEMENTS.      TO  PLAYBACK,    THESE   VALUES   ARE  SENT 

C  IN   THE   SAME   SEQUENCE  TO  THE  SERVOS  OF  ALL 

C  OF  THE  JOINTS. 

C  THIS   PROGRAM   PROVIDES   THE   OPERATOR  WITH  THREE 

C  '  OPTIONS   WITH   REGARDS   TO  THE   MANNER  OF  RECORDING 

C  THE  TRAJECTORY. 

C  (1)    A  CONTINUOUS    PATH   RECORDING  IDENTICAL 

C  TO  THE  ARM  PROGRAM.      BY  CONTINUOUS   PATH   IS  MEANT 

C  THAT  THE   POTENTIOMETER  VALUES  ARE  CLOCKED  IN 

C  WITH  A  CONSTANT  TIME   INTERVAL  THROUGHOUT  THE 

C  RECORDING  SESSION. 

C       .  (2)   A  POINT-TO-POINT  RECORDING.  HERE, 

C  THE   POTENTIOMETER  VALUES   ARE  READ   INTO  THE 

C  COMPUTER  ONLY  ON   COMMAND   FROM  THE  OPERATOR. 

C  THE    [CR]    IS   DEPRESSED  EACH  TIME  A  POINT   IS  TO 

C  BE  READ. 

C  (3) A  TWO   POINT  TRAJECTORY.      IN  THIS 

C  CASE  ONLY   THE   STARTING   POINT  AND  THE 

C  ENDING   POINT  ARE  RECORDED. 

C  DURING  THE   PLAYBACK  OF   BOTH   THE  CONTINUOUS 

C  PATH   AND  THE   POINT-TO-POINT  TRAJECTORIES,  AN 

C  INTERPOLATION   ROUTINE   IS   USED  TO  GENERATE 

C  ADDITIONAL   POINTS   BETWEEN   THE   RECORDED..  POI NTS  . 

C  THIS    PROVIDES   A  SMOOTHER   PLAYBACK  TRAJECTORY, 

C  AND  ALSO   PROVIDES   A  CONTROL  ON   THE  SPEED  THAT 

C  THE   TRAJECTORY   RUNS.      THIS   SPEED  CONTROL  IS 

C  ACCOMPLISHED   BY  SENDING   POINTS   TO  THE  ARM  AT 

C  A  CONSTANT   RATE   WHILE  THE   NUMBER  OF  ADDITIONAL 

C  POINTS   GENERATED   BETWEEN  THE   RECORDED  POINTS 

C  IS   A  FUNCTION  OF  THE   VOLTAGE   LEVEL  ON  INPUT 

C  CHANNEL   26 (THIS    IS   THE   POTENTIOMETER  ON  THE  FUNCTION 

C  KEYBOARD.      IT   IS   ALSO  THE   VALUE   Q  FOR  THE  SNW 

C  PROGRAM. ) . 

C  THE  TWO   POINT  TRAJECTORY   DOES   NOT  INCORPORATE 

C  THIS    INTERPOLATION   ROUTINE.      THUS  DURING 

C  PLAYBACK  THE  ARM  TRAVELS   AT   ITS  HIGHEST 

C  VELOCITY   FROM  THE  STARTING  TO  THE  ENDING 

C  POINT.      ITS  ABILITY  TO  PERFORM  THIS  TYPE 

C  OF  TRAJECTORY  TO  AN   END   POINT  WITHIN  ITS 

C  SPECIFIED  ACCURACY   IS   PART  OF  THE  ACCEPTANCE 

C  PROCEDURE. 

C 

Q  *********************************** 

c 
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IMPLICIT  INTEGER  (A-Z) 
COMMON/ARMaUF/INBUF (64) ,OUTBUF{64) 
DIMENSION   INCBUF(20ak}  ,7)  ,START(10)  ,FINI(10) 
REAL  NDEL,DBUF(2kJ) 

EQUI VALENCE (SWITCH, I NBUF( 27) ) , (SWIT2 , INBUF ( 28 ) ) 

C 

(J  ********** 

C  -2Z    IS   USED  AS  A  FLAG  TO  CAUSE   THE  PROGRAM 

C  TO  WAIT  AFTER  SE.NDING  THE   FIRST   POINT   IN  THE 

C  TRAJECTORY.      THE   ARM  SERVOS   CAN   NOW  BE   TURNED  ON 

C  AND  THE  ARM  ALLOWED  TO   POSITION    ITSELF.      IF  THE 

C  ARM  SERVOS   TO  THE   CORRECT   INITIAL   POSITION,  THEN 

C  A    [CR]    IS   TYPED  WHICH   SETS   ZZ=0   AND  CAUSES  THE 

C  TRAJECTORY  TO  BE   PLAYED  BACK. 

Q  ********** 

c 

3000  ZZ=-1 
C 

Q  ********** 

C  THE  SUBROUTINE  SETERR  WAS  EXPLAINED  IN  THE 

C  PROGRAM  SWTH. 

^  ********** 

C 

CALL  SETERR(3,-1) 

1  WRITE(6,444) 

444  FORMAT('    RECORD/PLAYBACK  DEMONSTRATION   PROGRAM '/3X ,/ 

1'    IF  YOU  WANT  A  CONTINUOUS  TRAJECTORY   TYPE+1'/ 

2'   IF  YOU  WANT  A  POINT  TO  POINT  TRAJECTORY—  TYPE  0 '/ 

3'   IF  YOU  WANT  A  TWO  POINT  TRAJECTORY   TYPE  -1') 

READ(6 ,445) PC 

445  F0RMAT(I5) 
IF(PC) 76,75,74 

C 

Q  ********** 

C   THIS   SECTION  OF  CODE   IS   FOR  THE  CONTINUOUS 

C  PATH  RECORDING  AND  IS   IDENTICAL  TO  THE  ARM 

C  PROGRAM. 

Q  ********** 

c 

74  WRITE ( 6 , 1000) 

READ(6,1100) FILFLG 

IF    (FILFLG   .N£.    1HY)G0T0  10 

WRITE (6 ,1010) 

DO  2  1=1,2000 

2  READ(2,1030  ,END=3)  (INCBUF(I,J)  ,J  =  1,7) 

3  IINC=I-1 
GOTO  3  00 

e 

10  LATCH=1 
IINC=1 
EOTHR=20 
EOTHRS=EOTHR 
WRITE (6,1020) 

C 

11  CALL  SANDF 
C 

Q  ********** 

C  THE  ARM  IS   NOW  POSITIONED  TO  THE  READ  VALUE 

C  NOW  CHECK  THE  SWITCH  AND  LATCH  TO  DECIDE 

C  THE  NEXT  COURSE  OF  ACTION. 

Q  ********** 
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IF(SWITCH  .LT.  -2000)  GOTO  ( 1 1 0  , 1 20  , 1 30  , 1 40  , 1 50 )  , LATCH 
GOTO   (21kJ, 220  ,  230  ,  240, 250)  , LATCH 

C 

ilM  WRITE(6,i030)EOTHR 

LATCH=2 
12W  EOTriRS=EOTHRS-l 

IF(EOTHRS)  121,121,11 

121  DO  122  JINC=1,7 

122  INCBUF (IINC, JINC) =OUTBUF ( J  I NC ) 
EOTHRS=EOTHR 

IINC=IINC+1 

IFdINC    .LE.    2«)0k))    GOTO  11 

LATCH   =  3 

rtRITE(6,1040) 

IINC=IINC-1 
13^  GOTO  11 

iAid  LATCH  =  5 

WRITE{6,11350) 

R£AD(6,1100) FILFLG 

WRITE(6,1220) 

1220  FORMAT (2X  FLIP  SWITCH   DOWN  TO  INITIALIZE  TRAJECTORY') 

150  GOTO  11 

C 

210  GOTO  11 

220  IItjC  =  IINC-l 

WRITE(6,1060) 
2  3  k)  latch  =  4 

240  GOTO  11 

250  IF(FILFLG   .N£.    1HY)G0T0  300 
WRITE(6,1070) 

DO  251   1  =  1  ,  IINC 

251  WRITE(2,1080)  (INCBUF(I,J)  ,J  =  1,7) 

C 

300  rtRITE{6  ,1090)  (INC6UF(1,J)  ,J  =  1,7)  ,IINC 

READ(6,1100) FILFLG 
310  KINC=IINC-1 
C 

Q  ********** 

C  THIS   IS  THE   INTERPOLATION   ROUTINE.  NDEL 

C  IS   SET  EQUAL  TO  THE  ABSOLUTE  VALUE  OF 

C  INPUT  CHANNEL   26   DIVIDED  BY   700.  THE 

C  NUMBER  ONE   IS  ADDED  TO  THIS   FRACTION  SO 

C  THAT  NDEL  WILL  NOT  BE   EQUAL  TO  ZERO (BECAUSE 

C  THE   INTEGER  CONVERSION  OF  A   FRACTION  LESS 

C  THAN  ONE  WILL  YIELD  A   ZERO.      THIS  RESULT 

C  IS   MULTIPLIED  BY   10  SO  THAT  THERE   IS  A 

C  MINIMUM  OF   10   POINTS  GENERATED  BETViEEN  EACH 

C  RECORDED   POINT.      THAT   IS,   THE  INTEGER 

C  VALUE  OF  NDEL   IS  THE  NUMBER  OF  POINTS  THAT 

C  WILL  BE  GENERATED  BETWEEN   EACH   PAIR  OF 

C  RECORDED  POINTS. 

Q  ********* 

c 

DO  320  I=1,KINC 
INBUF(26)=IABS(INBUF(26) ) 
NDEL=(INBUF(26)/700+l) *10 

C 


Q  ********* 

C  THE  DIFFERENCE   BETWEEN  TWO  SUCCESSIVE  RECORDED 

C  POINTS   IS   DIVIDED  BY  NDEL  TO  OBTAIN  A  DELTA 

C  (DaUF(J))    FOR  EACH  JOINT  J. 

Q  ********** 
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c 

DO  321  J-1,7 

321  DtiUF(J)  »  (INCBUF(I  +  1  ,  J)  -  INCBUFd  ,J)  )  /NOEL 

KDEL«NDEL 

C 

Q  ********** 

C  THIS   SECTION  OF  CODE   IS  REPEATED  FOR  EACH 

C  RECORDED   POINT.      IT  ADDS  THE   DELTA { DBUF ( J ) 

C  TO  THE  RECORDED  POINT  NDEL  TIMES, 

C  THEREBY  GENERATING  NDEL   POINTS  BETWEEN 

C  EACH  RECORDED  POINT 

Q  ********** 

C 

DO   322  K=1,KDEL 

DO   323  J=l,7 
323  OUTBUF(J)=INCflUF(I ,J) +DBUF(J) * (K-1) 

CALL  ARM0UT(7) 
3  22  CALL  ARMIN 

320  CONTINUE 

WRIT£(6,1221) 

1221  FORMAT (2X 'TO  REPEAT  THIS   TRAJECTORY,   TYPE    [CR]  .  '  , 
1    2X'T0  END   PROGRAM,    TYPE  A  +1',/ 

1    2X'T0  RETURN   TO  BEGINNING,   TYPE  A  -1.') 
READ(6,1222)TE 

1222  FORMAT (13) 

IF(TE)  3k)00  , 310  , 9300 

C 

1000  FORMAT('   ARM  TRAJECTORY  RECORD/PLAYBACK  DEMON 

ISTRATION  PROGRAM.'/'   DO  YOU  WANT  AN  OLD  TRAJECTORY?') 
1010         FORMAT('   READING  DATA  VALUES,   PLEASE  WAIT') 

1020          FORMAT('      FLIP  SWITCH  TO  UP  POSITION  TO  RECORD  TRAJECTORY') 
1030          FORMAT ('    BEGIN  RECORDING  EVERY    ',12,'  POINTS',/ 

1    '      FLIP  SWITCH  TO  DOWN   POSITION  TO  END  RECORDING.') 
1040        "FORMAT('    FILE  OVERFLOW,'/'   TRUNCATION  OF  RECORDED  TRAJECTORY 

1   OCCURED:    BE   EXTREMELY  CAREFUL  ON  PLAYBACK!') 
1050  FORMAT('    DO  YOU  WANT  TO  SAVE  THIS  TRAJECTORY?') 

1060          FORMAT('    END  RECORDING  TRAJECTORY.      FLIP  SWITCH  TO', 

1   2X'UP  POSITION  TO  PLAY  BACK.') 
1070  FORMAT('    PLEASE  WAIT  WHILE   FILE   IS   BEING  SAVED ' ) 

1080         FORMAT (7 (16, 2X) ) 

1090          FORMAT('   THE   INITIAL  ARM  POSITION   IS  '  ,/ ,  ( 7 ( I  6 , 2X)  )  ,/ 

1,  '   THERE  ARE ',15,'    POSITIONS   IN  THIS  TRAJECTORY',/ 

2,  '   TYPE  CARRIAGE  RETURN    [CR]    WHEN  READY.') 

C 

1100  FORMAT(Al) 
C 

Q  ********** 

C  THIS  SECTION  OF  CODE   IS   FOR  THE  TWO  POINT 

C  RECORDED  TRAJECTORY.      IT  FIRST  ASKS  THE 

C  OPERATOR  IF  A  PREVIOUSLY   RECORDED  TRAJECTORY 

C  IS   DESIRED.      IF  YES    (HUH=Y) ,   THE  OLD 

C  TRAJECTORY   IS   READ  FROM  THE  DISC. 

Q  ********** 

C 

76  WRITE(6,888) 

888  FORMAT ('    PROGRAM  FOR  TWO  POINT  TRAJECTORY'/ 

1    '    DO  YOU  WANT  AN  OLD  TRAJECTORY?') 

READ(6,446)HUH 
44  6  FORMAT (Al) 

IF (HUH.NE. IHY)    GO  TO  400 

READ (2, 10 80)  (START(I)  ,1  =  1,7)  ,(FINI(I)  ,1  =  1,7) 
GO  TO  4  32 

C 

Q  ********** 
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C   IF  A   NEW  TRAJECTORY   IS  TO  BE   RECORDED ( HUH=N ) 

C  THE   PROGRAM  INSTRUCTS  THE  OPERATOR  TO  FLIP  THE 

C  SWITCH  ON   INPUT  CHANNEL   27   TO  RECORD  THE 

C  BEGINNING  POINT. 

(2  ********** 

C 

Aliid  WRITE(6,431) 

431  FORMAT {2X 'FLIP  SWITCH  #1  TO  UP  POSITION  TO  RECORD  FIRST  POIN 
C 

^  ********** 

C  THE  PROGRAM  CYCLES   UNTIL  THE  SWITCH  IS 

C  FLIPPED. 

Q  ********** 

c 

44  7  COi>lTINUE  , 

CALL  SANOF 

IF (SWITCH. GT. -20^0)    GU  TO  447 
CALL  SANDF 
DO  555   I   =  1,7 
55  5  START(I) =OUTBUF(I) 

C 

Q  ********** 

C  THE   PROGRAM  INSTRUCTS  THE  OPERATOR  TO  FLIP 

C  THE  SWITCH  ON   INPUT  CHANNEL   28   TO  RECORD 

C  THE   END  POINT. 

Q  ********** 

c 

WRITE(6,448) 

448  FORMAT (2X' FLIP  SWITCH  12  TO  UP  POSITION  TO  RECORD  END  POINT' 
C 

Q  ********** 

C  AGAIN,   THE  PROGRAM  CYCLES   UNTIL  THE 

C  SWITCH   IS  FLIPPED. 

(2  ********** 

C 

449  CONTINUE 
CALL  SANDF 

IF (SWIT2.GT.-2000)   GO  TO  449 
CALL  SANDF 
DO  554   I   =  1,7 
554  FINI (I) =OUTBUF(I) 

C 

Q  ********** 

C  THE   PROGRAM  ASKS  THE  OPERATOR  IF  THIS 

C  RECORDED  TRAJECTORY   IS  TO  BE  SAVED  ON  THE  DISC. 

Q  ********** 

c 

WRITE(6,666) 
READ(6,661) HUH 
661  FORMAT(Al) 

666  FORMAT('   DO  YOU  WANT  TO  SAVE  THESE   POINTS   ON  DISC?') 

IF(HUH.NE.IHY)    GO  TO  665 

C 

Q  ********** 

C   IF  THE  ANSWER  IS   YES(HUH=Y)    THEN  THE  TRAJECTORY 
C  IS   WRITEN  ON  THE  DISC  BEFORE  PLAYBACK. 

Q  *********** 

c 

WRITE (2  ,1080)  (START (I)  ,1  =  1,7)  ,  (FINI (I)  ,1  =  1,7) 
665  WRITE(6,450) 

450  FORMAT ('    DO  YOU  WANT  TO  PLAYBACK?') 
READ(6,d89) HUH 
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THE 


889 

432 
452 

453 
C 
C 
C 
C 
C 
C 

c 

460 
454 


89t) 

891 

C 

C 

C  TH 

C 

C 

C 

C 

455 


892 


FORMAT (Al) 

IF(HUH.N£.1HY)   GO  TO  76 
WRITE(6,452) 

FORMAT('   TYPE  CARRIAGE  RETURN    (CR)    TO  INITIALIZE  ARM') 
READ{6 ,453) HUH 
FORMAT (I  5) 

********** 

BEGINNING  POINT  IS   LOADED  INTO  THE  OUTBUF 

REGISTERS,   SENT  TO  THE  ARM,   AND  THE  PROGRAM 
THEN  WAITS   FOR  A    [CR]    TO  CONTINUE. 
********** 


DO  4  5  4  I  =1,7 

OUTBUF (I) =START(I) 

CALL  ARM0UT(7) 

WRITE  (6  ,89k)) 

FORMAT ('   TYPE  CARRIAGE 

READ(6,891) HUH 

FORMAT (I  3) 


RETURN    [CR]    WHEN  READY') 


393 

C 

C 

C  IF 
C 

c 
c 
c 
c 
c 


********** 

E   END  POINT  IS   LOADED  INTO  THE  OUTBUF  REGISTERS 

AND  SENT  TO  THE  ARM.      THE  PROGRAM  ASKS  THE  OPERATOR 

IF   HE  WISHES  TO  REPEAT  THIS  TRAJECTORY. 
********** 

DO  455  1=  1,7 
OUTBUF(I) =FINI (I) 
CALL  ARM0UT(7) 
WRITE(6,892) 
FORMAT (2X 'TYPE 
1   2X'TYPE   +1  TO 
1   2X'TYPE   -1  TO 
READ(6 ,893) HUH 
FORMAT (13) 


[CR]    TO  REPEAT  THIS  TRAJECTORY',/ 
END  THE   PROGRAM ' ,/ 

RETURN  TO  THE   BEGINNING  OF  THE  PROGRAM.') 


********** 

THE  TRAJECTORY   IS  TO  BE  REPEATED,   THE  PROGRAM 

GOES  TO  STATEMENT  4  52  AND  WAITS  TO     OUTPUT  THE 
POINT.      IF  THE  TRAJECTORY  IS 
REPEATED,   CONTROL  RETURNS  TO  THE 
OF   THE  PROGRAM    (STATEMENT   3  0k)0)  . 
********** 


FIRST 


NOT  TO  BE 

BEGINNING 


C 
C 

C  THIS 

C 

C 

c 
c 
c 

C 
C 

c 

c 

75 


IF  (HUH)  3000  ,432  ,93tJ0 

********** 

SECTION  OF  CODE   IS   USED  TO  RECORD  THE 
POINT-TO-POINT  TRAJECTORY.      THIS   CODE  IS 
IDENTICAL  TO  THAT  USED   IN  THE  CONTINUOUS 
PATH  SECTION  WITH  THE   EXCEPTION  OF  A  READ 
STATEMENT    (AFTER  STATEMENT  517)    WHICH  CAUSES 
THE   PROGRAM  TO  STOP  AFTER  A  POINT   IS   READ  IN, 
TYPING  THE    [CR]    CAUSES  THE  NEXT  POINT  TO  BE 
RECORDED. 
********** 

CALL  SETERR(3,-1) 
WRITE(6,2000) 


47 


2000         FORMAT('    POINT  TO  POINT  DEMONSTRATION  PROGRAM'/ 

1    '   DO  YOU  WANT  AN  OLD  TRAJECTORY?') 

R£AD(6,1100) FILFLG 

IF   (FILFLG   .N£.    1HY)G0T0  500 
■  v<RITE(6,lO10) 

DO  501  1=1,2000 
501  READ(2,1080  ,END=503)  (INCBUF(I,J)  ,J  =  1,7) 

503  IINC=I-1 

GOTO  504 

C 

500  LATCri=l 
IINC=1 
EOTHR=2  0 
EOTHRS=EOTHR 
aRIT£(6,1020) 

C 

505  CALL  SANDF 

C 

Q  ********** 

C  THE  ARM  IS  NOW   POSITIONED  TO  THE  READ  VALUE 

C  NOW  CHECK  THE  SWITCH  AND  LATCH  TO  DECIDE 

C  THE  NEXT  COURSE  OF  ACTION. 

Q  ********** 

c 

IF(SWITCH   .LT.   -2000)    GOTO   ( 506  ,  507  ,  503  ,  509  ,  510)  ,  LATCH 
GOTO   (511 ,512,513  ,514  ,515)  , LATCH 

C 

506  WRITE (6,7270) 

7270         FORMAT (2X 'TYPE    [CR]    EVERY  TIME  A  POINT  IS  TO  BE  RECORDED.'/ 
1   2X'FLIP  SWITCH  TO  DOWN   POSITION  AND  TYPE  A  [CR]',/ 
1   2X'T0  END  THE  RECORDING.') 
LATCH=2 

507  E0THR3=E0THRS-1 
IF(EOTHRS)  516,516,505 

516  DO  517  JINC=1,7 

517  INCBUF(IINC,JINC)=OUTBUF(JINC) 
READ(6 ,913) FF 

913  FORMAT (13) 

EOTHRS=EOTHR 
IINC=IINC+1 

IFdINC    .LE.    2000)    GOTO  505 

LATCH  =  3 

WRITE(6,1040) 

IINC=IINC-1 

508  GOTO  505 

509  LATCH  =  5 
WRITE(6,1050) 
READ(6 ,1100) FILFLG 
WRITE(6,1220) 

510  GOTO  505 
C 

511  GOTO  505 

512  IINC=IINC-1 
WRITE(6,1060) 

513  LATCH  =4 

514  GOTO  505 

515  IF(FILFLG   .N£.    1HY)G0T0  504 
WRITE(6,1070) 

DO  518  I-1,IINC 
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518  WRITE(2,108k))  (INC6UF{I,J)  ,J-1,7) 
C 

504  WRITE(6,1090)  (INCBUF(1,J)  ,J-1,7)  ,IINC 

519  KINC=IINC-1 
ZZ  =  -1 

C  wRI'rE(6,7666) 

C7666  FORMAT (6X 'TYPE    [CK]    TO  RUN  TRAJECTORY  ) 

C  READ(6,7667) JII 

C7667  F0RMAT(I3) 
C 

Q  ********** 

C  THIS  IS  THE   INTERPOLATION   ROUTINE  AS 

C  EXPLAINED  IN  THE  CONTINUOUS  PATH 

C  SECTION. 

Q  ********** 

C 

DO  52kj  I  =  1,KINC 
INBUF(26) =IABS (INBUF(26)  ) 
NDEL=(INBUF(26)/7^0  +  l)  *lfc) 
DO  521  J=l,7 

5  21  DBUf  (J)  =  (INCBUF(I  +  1  ,J)  -INCBUFd  ,J)  )/NDEL 

KDEL=NDEL 

DO  522  K=1,KDEL 

DO  523  J=l,7 
523  OUTBUF(J) =INCBUF(I ,J) +DBUF(J) * (K-1) 

CALL  ARM0UT(7) 

IF(ZZ) 830,831,831 

830  READ(6,850) ZZ 
8  50  FORMAT (13) 

831  CONTINUE 
522  CALL  ARMIN 

520  CONTINUE 
WRITE (6,7631) 

7631  FORMAT (2X 'TYPE    [CR]    TO  REPEAT  THIS  TRAJECTORY',/ 
1   2X'TYPE   +1   TO  END  THE  PROGRAM',/ 

1  2X'TYPE  -1  TO  RETURN  TO  THE  BEGINNING  OF  THE  PROGRAM.') 
READ(6 ,7632) HUG 

7632  F0RMAT(I3) 

IF(HUG) 3000,504,9300 

C. 
C 

9300  CONTINUE 
END 
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